Sage Journals: Discover world-class research

Abstract

This paper discusses cooperative path-planning and tracking controller for autonomous vehicles using a distributed model predictive control approach. Mixed-integer quadratic programming approach is used for optimal trajectory generation using a linear model predictive control for path-tracking. Cooperative behaviour is introduced by broadcasting the planned trajectories of two connected automated vehicles. The controller generates steering and torque inputs. The steering and drive motor actuator constraints are incorporated in the control law. Computational simulations are performed to evaluate the controller for vehicle models of varying complexities. A 12-degrees-of-freedom vehicle model is developed and is subsequently linearised to be used as the plant model for the linearised model predictive control-based tracking controller. The model behaviour is compared against the kinematic, bicycle and the sophisticated high-fidelity multi-body dynamics CarSim model of the vehicle. Vehicle trajectories used for tracking are longitudinal and lateral positions, velocities and yaw rate. A cooperative obstacle avoidance manoeuvre is performed at different speeds using a co-simulation between the controller model in Simulink and the high-fidelity vehicle model in CarSim. The simulation results demonstrate the effectiveness of the proposed method.

Keywords

Applied mechanics automobile automotive control control theory dynamics dynamic modelling dynamic systems electric vehicle intelligent control mathematical modelling

Introduction

The main components of an autonomous vehicle are perception, planning and control. This paper discusses the planning and control aspects. For collision avoidance, generally a hierarchical control scheme is used with high level path-planner¹ and low level tracking controller.²

Significant research has been done in path-planning strategies using mixed-integer quadratic programming (MIQP),³ polynomials,⁴ B-splines,⁵ elastic bands⁶ and potential fields.⁷ In the research community, the path-planning problem has been widely studied for a single vehicle while in an autonomous driving environment multiple vehicles are present. A challenging research task is to achieve coordination by considering the trajectories of other autonomous vehicles. Schouwenaars et al.⁸ used an optimal path-planning approach for multiple vehicles based on MIQP. Frese and Beyerer⁹ compared several cooperative path-planning algorithms like tree search, elastic bands, priority-based approach, etc. for their computational times.

The lower level path-tracking controller ensures that a collision-free path is generated while a successful obstacle avoidance manoeuvre is achieved by making the vehicle precisely follow the planned trajectory. Trajectory tracking methods use fuzzy logic,¹⁰ proportional–integral–differential,¹¹ pure pursuit strategy,¹² linear quadratic regulator,¹³ sliding-mode control,¹⁴ robust control¹⁵ and model predictive control.¹⁶ MPC-based controllers easily handle actuator constraints and other uncertainties.¹⁷

Tracking controller requires vehicle model for mapping the reference trajectories. Researchers have widely used simplified models like kinematic model,¹⁸ bicycle model,¹⁹ planar lateral dynamics full car model²⁰ and models ignoring effects of roll, pitch and heave motions for developing the tracking controller.²¹ Moreover, when the chassis angular motions are considered, a general assumption is made that the vehicle rolls and pitches about the centre of gravity (CG), while in fact the vehicle rolls and pitches about roll and pitch axis respectively.²² For low-speed manoeuvres, simplified models capture the vehicle dynamics characteristics sufficiently well but at higher vehicle speeds and limits of handling there are significant differences in the predictions of these simplified.²³ These models are routinely linearised about an operating point but they deviate significantly from the nonlinear models at other operating points. To overcome this limitation, some researchers have used linear time-varying bicycle model but effect of out-of-plane motions is not captured.²⁴ Our contribution in this direction is to develop a linear time-varying model that captures the coupling effects of roll, pitch and heave motions.

Moreover, the limitation of simplified models is that only a limited number of vehicle states can be controlled. A vehicle model of intermediate complexity between a sophisticated commercial multibody dynamics model²⁵ and these simplified vehicle models is desirable for a real-time application. We developed a four-wheel vehicle model considering the effects of roll, pitch and unsprung mass motions, which can emulate real vehicle dynamic characteristics with low computational expense.

The main contribution of this paper is to develop a cooperative path-planning and tracking algorithm and evaluate it using models of varying complexities.

The cooperative path-planning architecture is based on a distributed MPC approach, where path-planner for each vehicle optimises its optimal trajectory according to the planned trajectories of their neighbouring vehicles that is broadcasted at each time step. In the tracking controller, a future input sequence of steering angle and drive torque is calculated for a predefined prediction horizon. The vehicle trajectories that are tracked are the positions and velocities in longitudinal and lateral directions.

The paper is organised as follows: methodology is described at first followed by the vehicle dynamics model. Four different vehicle models, namely, the kinematic model, bicycle model, a 12-degrees-of freedom (dof) model of intermediate complexity and a high-fidelity CarSim model are discussed in the vehicle model section. A relative comparison study of the vehicle trajectories obtained from various vehicle models for a single lane change manoeuvre is performed. The nonlinear vehicle model is subsequently linearised to be used in the MPC formulation for tracking. Next, the path-planning algorithm developed using model predictive control and MIQP is discussed followed by the formulation of the tracking controller. The results are discussed for a cooperative double lane change manoeuvre and finally conclusions are discussed.

Methodology

The proposed hierarchical scheme is shown in Figure 1. On the upper level, given a set of vehicles $\frac{V Δ}{= {1, 2, \dots, N_{v}}}$ , a path-planner for each vehicle generates the trajectory $X_{p}^{(i)}$ or $\frac{r_{p}^{(i)} Δ}{= [x_{p}^{(i)} y_{p}^{(i)} v_{x, p}^{(i)} v_{y, p}^{(i)}] T \in ℝ^{4} \forall i \in V}$ to follow a pre-defined route.

Figure 1.

Proposed algorithm of path planning and tracking.

To generate the collision-free reference trajectories, we assume that at every time step k, each vehicle i receives the future plans of the others agents $X_{p}^{(j)}$ or $\frac{r_{p}^{(j)} Δ}{= [x_{p}^{(j)} y_{p}^{(j)} v_{x, p}^{(j)} v_{y, p}^{(j)}] T \in ℝ^{4}}$ over a finite horizon, where j denotes the jth surrounding vehicle.

On the lower level, the tracking controller tracks the trajectory (i.e. a set of way-points) $r_{p}^{(i)}$ generated by the path-planning layer, which is then used as the reference trajectory $r_{t}$ . The subscripts p and t are used to represent the trajectories for the path-planner and the controller, respectively. The tracking controller computes the drive torque T and the steering wheel angle $δ_{sw}$ necessary to control the vehicle. Note here that this is different from the road wheel angle δ, which can be obtained by dividing the steering wheel angle with the steering ratio. The assumptions made in this approach are that the route is known beforehand, i.e. a start and a goal position and the current state of the vehicle is estimated based on the measurements from the vehicle sensors and are available for feedback.

MPC-based path-tracking algorithm is presented to follow desired speeds while allowing to temporary deviate from desired paths in Funke et al.²⁶ The underlying idea of optimisation with both velocity and position states is quite useful.²⁷ In addition to tracking of the longitudinal and lateral positions and velocities, we also track the yaw rate reference. Therefore, the vehicle control output vector is given by

Y = {\begin{matrix} x & y & v_{x} & v_{y} & ω \end{matrix}}

Vehicle model

Four vehicle models are described in the following sections.

Kinematic model

The kinematic vehicle model is shown in Figure 2(a). The inputs to the model are vehicle speed v and steering angle δ. From Figure 2(a), the longitudinal and lateral velocities of the centre of mass G are given by

\overset{\cdot}{x} = v cos (ψ + β); \overset{\cdot}{y} = v sin (ψ + β)

(1)

Figure 2.

(a) Kinematic model; (b) bicycle model.

This model assumes no-slip condition. This implies that the velocities of each wheel points in the same direction of wheel and are equal. As there is no-slip condition, there lies an instantaneous centre of rotation ‘O’ about which the vehicle rotates and the rotation rate of the vehicle Ω is given by

Ω = \overset{\cdot}{ψ} = \frac{v}{R}; R = \frac{l_{r}}{sin β}

(2) where R is the instantaneous radius of rotation. Eliminating R we obtain the governing equations of motion as

\overset{\cdot}{x} = v cos (ψ + β); \overset{\cdot}{y} = v sin (ψ + β); \overset{\cdot}{ψ} = \frac{v}{l_{r}} sin β

From Figure 2(a)

Δ OBG, OBA : β = tan - 1 (\frac{l_{r}}{l} tan δ) < label > (3) < / label >

Bicycle model

The bicycle model is shown in Figure 2(b).

Under small steering angle approximation, the front and rear slip angles are given by

α_{f} = β + \frac{Ω l_{f}}{V} - δ; α_{r} = β - \frac{Ω l_{r}}{V}

(4)

The lateral acceleration of the CG a_y can be obtained from the fixed and rotating coordinate system representation as

a_{y} = V \overset{\cdot}{β} + \overset{\cdot}{V} β + Ω V

(5)

The lateral force balance equations for the bicycle model (again under small angle approximation) is given by

F_{yf} + F_{yr} = m a_{y} = m (V \overset{\cdot}{β} + \overset{\cdot}{V} β + Ω V)

(6)

F_yf and F_yr are the front and rear cornering forces given by

F_{yf} = - C_{α_{f}} α_{f}; F_{yr} = - C_{α_{r}} α_{r}

(7)

From equations (4), (6) and (7) we obtain

\begin{matrix} m (V \overset{\cdot}{β} + \overset{\cdot}{V} β + Ω V) = - C_{α_{f}} (β + \frac{Ω l_{f}}{V} - δ) \\ - C_{α_{r}} (β - \frac{Ω l_{r}}{V}) \end{matrix}

(8)

The longitudinal force balance (under small steering angle δ approximation) yields

m (\overset{\cdot}{V} - Ω V β) = F_{xr} - F_{yf} δ - F_{d} - F_{r}

(9)

F_d is the aerodynamic drag and F_r is the rolling resistance given by

F_{d} = \frac{1}{2} ρ C_{d} A u^{2}; F_{r} = C_{r} W = C_{r} mg

(10)

C_d is the drag coefficient, C_r is the coefficient of rolling resistance, A is the car frontal area and W is the vehicle weight. For simplicity we assume no longitudinal slip, which implies that, $F_{x} = \frac{T}{r_{w}}$ .

Finally, yaw moment balance about the CG ‘G’ yields

\begin{matrix} I_{z} \overset{\cdot}{Ω} = l_{f} F_{yf} - l_{r} F_{yr} \\ = - C_{α_{f}} (β + \frac{Ω l_{f}}{V} - δ) l_{f} + C_{α_{r}} (β - \frac{Ω l_{r}}{V}) l_{r} \end{matrix}

(11)

Equations (8) to (11) represent governing equations of motion.

12-dof model

The 12-dof model is shown in Figure 3.

Figure 3.

12-degrees-of-freedom vehicle model.

This model consists of three translational dof corresponding to longitudinal, lateral and vertical motion; three rotational dof corresponding to roll, pitch and yaw; four dof (one for each wheel) for wheel jounce; two dof for the angular rotation of rear left and right wheels. Note that in our present study, we have considered a vehicle with rear wheel drive and so we are interested only in the rear wheel velocities for longitudinal slip calculation as described in the section on the wheel model.

Coordinate system

It is convenient to write the equations of motion in noninertial (moving) reference frame $O (x, y, z)$ instead of the inertial frame $C (X, Y, Z)$ (see Figure 4). The fixed or inertial coordinate system is attached to the ground with X-axis in the forward, Y-axis in the lateral and Z-axis in the vertical direction such that a right-handed Cartesian coordinate system is formed. The noninertial or rotating coordinate system is attached to the vehicle, which is rotating about the z-axis with angular velocity Ω (see Figure 4). The x-axis points forwards and lies both in the ground plane and in the plane normal to the ground that passes through the so-called roll axis. The y-axis points to the left and the z-axis points upwards and passes through the CG at zero roll and pitch.²⁸

Figure 4.

Fixed and rotating coordinate systems.

For deriving all equations of motion, small angle approximation for roll, pitch and wheel steering angle is considered. Eventually, the nonlinear equations of motion are linearised and are represented in the state-space form.

Planar motions

In this section, we study the planar motions of the chassis in x, y directions and yaw about z-axis.

Longitudinal and lateral dynamics:

The absolute position of the vehicle CG is given by

r = r_{O} + r_{G}

(12)

The position vector $r_{G}$ is given by

r_{G} = d_{p} θ \overset{ࢳ}{i} - d_{r} φ \overset{ࢳ}{j} + h \overset{ࢳ}{k}

(13)

Since $O (x, y, z)$ is a rotating coordinate system with angular velocity $Ω = Ω \overset{ࢳ}{k}$ , therefore the absolute velocity of the CG is given by

\overset{\cdot}{r} = v = {\overset{\cdot}{r}}_{O} + {\overset{\cdot}{r}}_{G} + Ω \times r_{G}

(14)

For small pitch and roll angles, above equation reduces to

v = (v_{x} + d_{p} \overset{\cdot}{θ} + d_{r} φ Ω) \overset{ࢳ}{i} + (v_{y} - d_{r} \overset{\cdot}{φ} + d_{p} θ Ω) \overset{ࢳ}{j}

(15) where v_x and v_y are the velocities of the moving coordinate system. Correspondingly, the acceleration is given by

a = \overset{\cdot}{v} + Ω \times v

(16) which yields

\begin{matrix} a = ({\overset{\cdot}{v}}_{x} + d_{p} \overset{··}{θ} + 2 d_{r} \overset{\cdot}{φ} Ω + d_{r} φ \overset{\cdot}{Ω} - v_{y} Ω - d_{p} θ Ω^{2}) \overset{ࢳ}{i} \\ + ({\overset{\cdot}{v}}_{y} - d_{r} \overset{··}{φ} + 2 d_{p} \overset{\cdot}{θ} Ω + d_{p} θ \overset{\cdot}{Ω} + v_{x} Ω + d_{r} φ Ω^{2}) \overset{ࢳ}{j} \end{matrix}

(17)

Sum of the external forces is equal to the rate of change of linear momentum P . Applying force balance equation $\sum F = P^{.}$ , and taking x and y components (planar motion) yield

\sum F_{x} = m a_{x}; \sum F_{y} = m a_{y}

(18) where a_x, a_y are

\overset{ࢳ}{i}, \overset{ࢳ}{j}

components of the acceleration vector a .

For small values of wheel steering angle δ and side-slip angles α_i, the equations can be linearised and the resultant forces in x and y directions are given by (There is a small difference between left and right wheel steering angles for normal handling situations. Using the same steering angle for both inner and outer wheels is an acceptable approximation for handling analyses.²⁹)

\begin{matrix} \sum F_{x} = F_{x 3} + F_{x 4} - (F_{y 1} + F_{y 2}) δ - F_{d} - \sum F_{rr, i}; \\ \sum F_{y} = F_{y 1} + F_{y 2} + F_{y 3} + F_{y 4} \end{matrix}

(19)

It is important to note here that the vehicle used for this study has rear wheel drive and therefore the longitudinal forces are generated by rear tyres only (see Figure 3).

F_d is the aerodynamic drag and $F_{rr, i}$ is the rolling resistance acting on each wheel (i) given by

F_{d} = \frac{1}{2} ρ C_{d} {Av}_{x}^{2}; F_{rr, i} = C_{rr} F_{zi}

(20) where C_d is the drag coefficient, C_rr is the coefficient of rolling resistance, A is the car frontal area and F_zi is the normal load on the tyre (see ‘Suspension dynamics’ section) with index

i \in {1 - 4}

corresponding to front left/right and rear left/right wheel, respectively.

Yaw dynamics:

To study the yaw dynamics, we begin with the calculation of the angular momentum. The angular momentum $H_{G}$ around CG G of a rigid body can be written as

\begin{matrix} H_{G} = I \times Ω = (I_{xx} ω_{x} - I_{xy} ω_{y} - I_{xz} ω_{z}) \overset{ࢳ}{i} \\ + (- I_{yx} ω_{x} + I_{yy} ω_{y} - I_{yz} ω_{z}) \overset{ࢳ}{j} \\ + (- I_{zx} ω_{x} - I_{zy} ω_{y} + I_{zz} ω_{z}) \overset{ࢳ}{k} \end{matrix}

(21)

The elements I_xx, I_yy and I_zz are the inertia moments and I_xy, I_yz and I_zx are inertia products and are the off-diagonal terms of the inertia tensor. The components of the angular velocity $Ω = [ω_{x} ω_{y} ω_{z}]$ in x, y and z direction are $ω_{x} = \overset{\cdot}{φ}, ω_{y} = \overset{\cdot}{θ}$ and $ω_{z} = Ω$ . Considering sprung mass to be symmetric in x–z plane, $I_{xy} = I_{yz} = 0$ and I_zx = I_xz in the above equation yields

H_{G} = (I_{xx} \overset{\cdot}{φ} - I_{xz} Ω) \overset{ࢳ}{i} + I_{yy} \overset{\cdot}{θ} \overset{ࢳ}{j} + (I_{zz} Ω - I_{zx} \overset{\cdot}{φ}) \overset{ࢳ}{k}

(22)

For calculating the angular momentum $H_{O}$ about any arbitrary point O, we consider the transfer theorem as

H_{O} = H_{G} + r_{G} \times P

(23)

Finally the moment balance equation is given by

\sum M = {\overset{\cdot}{H}}_{O}

(24)

Considering the moment balance in z direction about O in Figure 5 we obtain

\begin{matrix} (I_{zz} + m_{s} ((d_{r} φ) 2 + (d_{p} θ) 2)) \overset{\cdot}{Ω} - I_{xz} \overset{··}{φ} \\ = \sum M_{z} - m_{s} (d_{r} φ a_{x} + d_{p} θ a_{y}) \end{matrix}

(25)

Figure 5.

Lateral dynamics model.

The external moment $\sum M_{z}$ is given by

\begin{matrix} \sum M_{z} = (F_{y 1} + F_{y 2}) l_{f} - (F_{y 3} + F_{y 4}) l_{r} \\ + (F_{x 4} - F_{x 3}) \frac{t}{2} + (F_{y 1} + F_{y 2}) δ \frac{t}{2} \end{matrix}

(26)

It is important to note that the input given by the driver is the steering wheel angle $δ_{sw}$ and in order to obtain the road wheel angle δ, the steering wheel angle has to be divided by the steering ratio (SR). On substituting the values of $\sum M_{z}$ from above equation and the values of a_x and a_y from equation (17), simplifying and neglecting small terms we obtain

\begin{matrix} I_{zz} \overset{\cdot}{Ω} - I_{xz} \overset{··}{φ} + m_{s} [d_{r} φ ({\overset{\cdot}{v}}_{x} - v_{y} Ω) + d_{p} θ ({\overset{\cdot}{v}}_{y} + v_{x} Ω)] \\ = (F_{y 1} + F_{y 2}) l_{f} - (F_{y 3} + F_{y 4}) l_{r} \\ + (F_{x 4} - F_{x 3}) \frac{t}{2} + (F_{y 1} + F_{y 2}) δ \frac{t}{2} \end{matrix}

(27)

Finally, the in-plane motions can be studied using longitudinal and lateral force balance equations in (18) and yaw moment balance equation in (27).

Out-of-plane motions

In this section, out-of-plane motions: roll, pitch, heave and wheel jounce motions are analysed.

Roll dynamics:

The vehicle rolls about the roll axis passing through the front and rear suspension roll centres.³⁰ The distance between the CG and the roll axis (roll centre) is the roll moment arm d_r (see Figure 6). The roll centre height varies with suspension travel but for the sake of simplicity this variation is neglected.

\begin{matrix} (I_{xx} + m_{s} d_{r}^{2}) \overset{··}{φ} + (I_{zz} - I_{yy}) \overset{\cdot}{θ} Ω - I_{xz} \overset{\cdot}{Ω} \\ = M_{x} + m_{s} a_{y} d_{r} + m_{s} g d_{r} φ \\ where, M_{x} = M_{xs} + M_{xu} \end{matrix}

(28)

Figure 6.

Roll dynamics model.

M_xs and M_xu are sprung mass and unsprung mass roll moments given by

\begin{matrix} M_{xs} = M_{φ f} + M_{φ r} = - (K_{φ f} + K_{φ r}) φ - (C_{φ f} + C_{φ r}) \overset{\cdot}{φ}, \\ M_{xu} = \sum_{i = 1}^{4} F_{yi} h_{r}; \\ K_{φ f, r} / C_{φ f, r} = \frac{t^{2}}{2} K_{f, r} / C_{f, r} \end{matrix}

(29)

$K_{φ f, r}$ and $C_{φ f, r}$ are front/ rear roll stiffness and roll damping, respectively. From equations (28) through (29) we obtain

\begin{matrix} (I_{xx} + m_{s} d_{r}^{2}) \overset{··}{φ} + (I_{zz} - I_{yy}) \overset{\cdot}{θ} Ω - I_{xz} \overset{\cdot}{Ω} - m_{s} d_{r} (a_{y} + g φ) \\ + (K_{φ f} + K_{φ r}) φ + (C_{φ f} + C_{φ r}) \overset{\cdot}{φ} - \sum_{i = 1}^{4} F_{yi} h_{r} = 0 \end{matrix}

(30)

Pitch dynamics:

As similar to the roll dynamics, the vehicle pitches about the pitch centre P in Figure 3. The distance between the CG and the pitch centre is the pitch moment arm d_p (see Figure 7). Considering the pitch moment balance (small pitch angle approximation) about the pitch centre P we obtain

\begin{matrix} (I_{yy} + m_{s} d_{p}^{2}) \overset{··}{θ} + (I_{xx} - I_{zz}) \overset{\cdot}{φ} - m_{s} g d_{p} θ + m_{s} a_{x} d_{p} θ Ω \\ = (F_{z 3} + F_{z 4}) l_{r} - (F_{z 1} + F_{z 2}) l_{f} - (F_{x 3} + F_{x 4}) h_{p} \end{matrix}

(31)

Figure 7.

Pitch dynamics model.

Suspension dynamics:

In this part, we will study the heave motions of the chassis and wheel jounce. To study this, we will consider the 7-dof ride model of the car as shown in Figure 8.

Figure 8.

7-dof ride model. In the present study, the vehicle moves on a flat road so ground deflections are not considered.

Considering the force balance in z direction, the governing equation for the chassis heave motion z is given by

\begin{matrix} m_{s} a_{z} = \sum F_{z} = - \sum_{i = 1}^{4} (K_{i} (z_{i} - z_{ui}) + C_{i} ({\overset{\cdot}{z}}_{i} - {\overset{\cdot}{z}}_{ui})), \\ \forall i \in {1 - 2}, K_{i}, C_{i} = K_{f}, C_{f} else K_{r}, C_{r} \end{matrix}

(32) where the corner displacements z_i are given by

\begin{matrix} z_{1} = z + \frac{t}{2} φ - l_{f} θ; z_{2} = z - \frac{t}{2} φ - l_{f} θ; \\ z_{3} = z + \frac{t}{2} φ + l_{r} θ; z_{4} = z - \frac{t}{2} φ + l_{r} θ \end{matrix}

Considering the force balance in z direction for each unsprung mass we obtain the wheel jounce z_ui as

\begin{matrix} m_{u 1} {\overset{··}{z}}_{u 1} = K_{f} (z_{1} - z_{u 1}) + C_{f} ({\overset{\cdot}{z}}_{1} - {\overset{\cdot}{z}}_{u 1}) - K_{t} z_{u 1} - C_{t} {\overset{\cdot}{z}}_{u 1}; m_{u 2} {\overset{··}{z}}_{u 2} = K_{f} (z_{2} - z_{u 2}) + C_{f} ({\overset{\cdot}{z}}_{2} - {\overset{\cdot}{z}}_{u 2}) - K_{t} z_{u 2} - C_{t} {\overset{\cdot}{z}}_{u 2}; \end{matrix}

(33)

\begin{matrix} m_{u 3} {\overset{··}{z}}_{u 3} = K_{r} (z_{3} - z_{u 3}) + C_{r} ({\overset{\cdot}{z}}_{3} - {\overset{\cdot}{z}}_{u 3}) - K_{t} z_{u 3} - C_{t} {\overset{\cdot}{z}}_{u 3}; \\ m_{u 4} {\overset{··}{z}}_{u 4} = K_{r} (z_{4} - z_{u 4}) + C_{r} ({\overset{\cdot}{z}}_{4} - {\overset{\cdot}{z}}_{u 4}) - K_{t} z_{u 4} - C_{t} {\overset{\cdot}{z}}_{u 4} \end{matrix}

The vertical forces on the tyres is given by

\begin{matrix} F_{zi} = mg (\frac{l - l_{i}}{l}) - K_{t} z_{ui} - C_{t} {\overset{\cdot}{z}}_{ui}; \\ \forall i \in {1 - 4}; l_{1, 2} = l_{f}, l_{3, 4} = l_{r} \end{matrix}

(34)

Wheel model:

The longitudinal and lateral forces are generated by the wheels. As pointed out earlier, we have considered a rear wheel driven vehicle. As a result, the longitudinal force is generated only by the rear wheels while the lateral forces are generated by all the four wheels.

The first step in calculating the forces generated by a tyre, is to compute the longitudinal slip s_xi, and lateral slip s_yi. To calculate wheel slip we require wheel corner velocities and wheel angular speeds, which are obtained from Figure 9.

Figure 9.

(a) Corner velocities; (b) wheel angular dynamics.

The wheel corner velocities depend on vehicle velocity, yaw rate and geometrical parameters of the chassis as

\begin{matrix} v_{x 1, 3} = v_{x} - Ω \frac{t}{2}; v_{x 2, 4} = v_{x} + Ω \frac{t}{2}; \\ v_{y 1, 2} = v_{y} + Ω l_{f}; v_{y 3, 4} = Ω l_{r} - v_{y} \end{matrix}

(35)

The theoretical slip quantities are defined as³¹

s_{xi} = \frac{v_{xi} - ω_{i} r_{w}}{ω_{i} r_{w}}; s_{yi} = \frac{v_{yi}}{ω_{i} r_{w}} = tan α_{i} (1 + s_{xi})

(36)

The overall or total slip at each tyre is defined by

s_{i} = \sqrt{s_{xi}^{2} + s_{yi}^{2}}

(37)

Next the total friction coefficient using Pacejka's ‘magic formula (MF)’ is given by

μ (s_{i}) = D sin (C ta n^{- 1} (B s_{i}))

(38)

Assuming symmetric tyre characteristics, the total friction force for each tyre lies within the friction circle and the longitudinal and lateral tyre forces are given by³²

\begin{matrix} F_{xi} = \frac{s_{xi}}{s_{i}} μ (s_{i}) F_{zi}; i \in {3 - 4} \\ F_{yi} = \frac{s_{yi}}{s_{i}} μ (s_{i}) F_{zi}; i \in {1 - 4} . \end{matrix}

(39)

In the above equations, the wheel angular velocity $ω_{i} = {\overset{\cdot}{Θ}}_{i}$ is calculated using the driving torque and the longitudinal force generated in the contact between the road and the tyre as shown in Figure 9(b).

I_{w} {\overset{\cdot}{ω}}_{i} = \frac{T}{2} - F_{xi} r_{w}; i \in {3 - 4}

(40)

In our case, the engine torque T is equally distributed to both the rear wheels as the vehicle considered for study is a rear wheel driven car with an open differential of gear ratio 1. As a result, only the longitudinal slips of rear wheels come into picture $(i \in {3 - 4})$ while the lateral forces are generated by all the four wheels $(i \in {1 - 4})$ . This brings us to an end of vehicle modelling. These nonlinear vehicle models are finally linearised to be used in the linear MPC based tracking controller. The linearisation procedure is described in the ‘Linearisation’ section.

In addition to these we have developed a sophisticated multi body dynamics vehicle model in CarSim which represents a real world vehicle and will be eventually used to evaluate the tracking controller.

CarSim model:

The CarSim modeling interface is shown in Figure 10. The vehicle model was built for a prototype Westfield Sports Car.³³ The weight, inertia, CG location of the vehicle, suspension and steering properties were obtained from measurements. The vehicle parameters used in this model are reported in Table 1. The vehicle is equipped with double wishbone suspensions in both front and rear axles with a rigid roll-cage chassis. The values for spring and damper parameters, roll centre height, track width, spin inertias and compliance coefficients were used to build the suspension model. The suspension kinematics and compliance characteristics were fed as look-up tables in CarSim to model the suspension characteristics. An approximate linear relationship between handwheel and roadwheel steer angle is found for the range of $\pm 3 π$ (three and a half turns). The steering ratio is found to be equal to 13. Also, other parameters like caster, kingpin angle, lateral offset were modelled based on measurements. These details were obtained from the manufacturer and not discussed here for the sake of brevity. A magic formula based tyre model was used and the tyre vertical, longitudinal and lateral characteristics were inputted by means of look-up charts, which were obtained from Westfield Cars. The inputs to the CarSim model are engine troque and steering wheel angle. The default powertrain was replaced by an external drive torque command and the traditional rack and pinion steering system was replaced by external steering command coming from the tracking controller.

Figure 10.

CarSim modeling interface.

Table 1.

Vehicle parameters used to develop the mathematical model.

Notation	Description	Value	Notation	Description	Value
m	Vehicle mass	950 kg	l	Wheel base	2350 mm
m _s	Sprung mass	850 kg	l _f	Wheel base front	1350 mm
m _u	Unsprung mass (each)	25 kg	l _r	Wheel base rear	1000 mm
C _d	Drag coefficient	0.3	t	Track width	1500 mm
A	Car frontal area	1.6 m²	K_f, K_r	Spring stiffness front, rear	45, 60 N/mm
SR	Steering ratio	13	C_f, C_r	Shock damping front, rear	3.5, 4 N s/mm
C _rr	Rolling coefficient	0.01	K _t	Tyre stiffness	250 N/mm
I _xx	Roll inertia	325 kg m²	C _t	Tyre damping	0.75 N s/mm
I _yy	Pitch inertia	1000 kg m²	I _w	Wheel inertia	2.1 kg m²
I _zz	Yaw inertia	1200 kg m²	r _w	Wheel radius	325 mm
I _xz	Product of inertia	140 kg m²	B, C, D	Tyre parameters (MF)	7, 1.6, 1
d _r	Roll moment arm	125 mm	h _r	Roll centre height	200 mm
d _p	Pitch moment arm	150 mm	h _p	Pitch centre height	175 mm
$C_{α_{f}}$	Tyre cornering stiffness (front)	4.4 kN/rad	$C_{α_{r}}$	Tyre cornering stiffness (rear)	6 kN/rad

The mathematical model so developed has 57 dof, which in turn solves 114 first-order differential equations to calculate the vehicle responses. This model is exported as an S-function and in Simulink and a co-simulation interface is setup to assess the performance of the tracking controller.

Model comparisons

So far we developed four vehicle models, namely, kinematic model, bicycle model, 12-dof model and the CarSim model. These models differ from each other in terms of complexity. The kinematic model is the simplest with only three states. It does not take into account tyre compliance and is geometry based. The bicycle model is a further correction of the kinematic model with tyre lateral compliance taken into account. The 12-dof model takes suspension compliance, out-of-plane motions and wheel jounce into account and is a model of intermediate complexity as opposed to the sophisticated high-dof CarSim model.

The predictions of these vehicle models differ significantly at high lateral accelerations. To demonstrate this, the vehicle trajectories obtained from various models for a lane change manoeuvre at 80 km/h are compared in Figure 11. A sinusoidal steering wheel angle of 26 ° is given to achieve a lane change manoeuvre. The vehicle path X – Y, lateral velocity v_y and yaw rate Ω trajectories are obtained for each model.

Figure 11.

Responses of different models for a sinusoidal steering wheel angle input for a lane change maneuver at 80 km/h.

From Figure 11, it is evident that the predictions from Kinematic model substantially differ from that of the sophisticated CarSim model. Bicycle model gives a better prediction of the yaw rate response but it fails to give a representative prediction of lateral velocity and correspondingly the lateral position trajectories. The 12-dof proposed model is midway in complexity as opposed to the CarSim model and it fairly captures all the necessary vehicle dynamic handling characteristics.

For subsequent implementation, we have chosen 12-dof model and will compare its response against the bicycle model after integrating path-planning and tracking controller. Now, we discuss the linearisation of these models.

Linearisation

The governing equations of the vehicle dynamics model developed in the earlier section, can be written in first-order form as

X^{.} = f (X, U)

(41) where X is the state vector and U is the control vector.

Next, the above nonlinear model can be linearised by expanding around the equilibrium point ( $X_{e}, U_{e}$ ) as

{X_{e}}^{.} + Δ \overset{\cdot}{X} = f (X_{e}, U_{e}) + \frac{\partial f}{\partial X} Δ X + \frac{\partial f}{\partial U} Δ U

(42)

The resultant linearised model is given by

\overset{\cdot}{X} = A X + B U; Y = C X

(43) where the time-variant linear system matrices (state matrix A and input matrix B) are obtained using Jacobian matrix as (To obtain the linearisation point (

X_{e}, U_{e}

), equation (41) is symbolically written in Matlab to obtain Jacobian matrices using Jacobian function.)

\frac{A Δ}{= \frac{\partial f}{\partial X} |_{\frac{\begin{matrix} X = X_{e} \\ U = U_{e} \end{matrix} \in ℝ^{n \times n}; B Δ}{= \frac{\partial f}{\partial U} |_{\begin{matrix} X = X_{e} \\ U = U_{e} \end{matrix} \in ℝ^{n \times m}}}}}

(44) where n is the number of states and m is the number of control inputs. In equation (43), the state vector X , the control vector U and the output vector Y are given by

For bicycle model, $X = {\begin{matrix} x & y & Ψ & v_{x} & v_{y} & Ω \end{matrix}}^{T} .$

For 12-dof model

\begin{matrix} X = {\begin{matrix} x & y & z & φ & θ & Ψ & z_{1} & z_{2} & z_{3} & z_{4} & v_{x} \\ v_{y} & v_{z} & \overset{\cdot}{φ} & \overset{\cdot}{θ} & Ω & ω_{3} & ω_{4} & {\overset{\cdot}{z}}_{1} & {\overset{\cdot}{z}}_{2} & {\overset{\cdot}{z}}_{3} & {\overset{\cdot}{z}}_{4} \end{matrix}}^{T} \\ Y = {\begin{matrix} x & y & v_{x} & v_{y} & Ω \end{matrix}}^{T}; U = {\begin{matrix} δ_{sw} & T \end{matrix}}^{T} \end{matrix}

Output matrix, $C \in ℝ^{o \times n}$ (o = 5, the number of outputs). To test the path-planning and tracking algorithm, a scenario of vehicle running at $v_{x} =$ 60 km/h is considered. Correspondingly, the linearisation point $(X_{e}, U_{e})$ is:

For bicycle model

X_{e} = {\begin{matrix} 0 & 0 & 0 & v_{x} & 0 & 0 \end{matrix}}^{T}; U_{e} = {\begin{matrix} δ_{sw} 0 \end{matrix}}^{T}

For 12-dof model

\begin{matrix} X_{e} = {\begin{matrix} {[0]}_{1 \times 10} & v_{x} & {[0]}_{1 \times 5} & \frac{v_{x}}{r_{w}} & \frac{v_{x}}{r_{w}} & {[0]}_{1 \times 4} \end{matrix}}^{T}, \\ U_{e} = {\begin{matrix} δ_{sw} 0 \end{matrix}}^{T} \end{matrix}

The ground wheel angle δ is obtained by dividing the handwheel steering angle δ_sw by the steering ratio (SR).

Finally, the state matrices are discretised to obtain a discrete time state-space model as

\begin{matrix} X (k + 1) = A_{d} X (k) + B_{d} U (k), \\ Y (k) = C_{d} X (k) \end{matrix}

(45)

For bicycle model: $A_{d} \in ℝ^{6 \times 6}, B_{d} \in ℝ^{6 \times 2}$ and $C_{d} \in ℝ^{5 \times 6}$ .

For 12-dof model: $A_{d} \in ℝ^{22 \times 22}, B_{d} \in ℝ^{22 \times 2},$ $C_{d} \in ℝ^{5 \times 22}$ .

These discrete time matrices are obtained using the zero-order hold (ZOH) discretisation method in Matlab, with an integration step of $Δ T$ = 0.05 s.

Path-planning

The path-planner generates optimal trajectories using a mixed-integer quadratic programming approach with collision avoidance constraints in a receding-horizon fashion.³⁴

Collision avoidance

Collision avoidance between two autonomous vehicles is ensured by following linear inequality convex constraints

| x^{p_{1}} (k) - x^{p_{2}} (k) | \geq Δ_{e_{x}} and | y^{p_{1}} (k) - y^{p_{2}} (k) | \geq Δ_{e_{y}}

(46)

Big-M method is used to rewrite the collision avoidance constraints in terms of binary variables $b_{r} (k)$ as

\begin{matrix} x^{p_{i}} (k) - x^{p_{j}} (k) \geq Δ_{e_{x}} - M b_{r} (k), r \in {1 - 2} \\ y^{p_{i}} (k) - y^{p_{j}} (k) \geq Δ_{e_{y}} - M b_{r} (k), r \in {3 - 4} \\ \sum_{r = 1}^{4} b_{r} (k) \leq 3, M >> 0 alargenumber \end{matrix}

(47)

The binary variables $b_{r} (k) \in {0, 1}$ are additional decision variables in the optimisation problem added at each time step k. If b_r = 0, then the ith constraint in equation (47) is activated. However, if b_r = 1, then that constraint is relaxed.

Remark 1

To achieve obstacle avoidance with static objects, we incorporate the same constraints set as described in equation (47) replacing the planned trajectories $\frac{r_{p}^{(j)} Δ}{= [x_{p}^{(j)} y_{p}^{(j)}] T \in ℝ^{2}}$ with the coordinates of obstacles $O \in ℝ^{2}$ .

The location of the obstacles is known. This assumption can be satisfied by using cameras, for example light detection and ranging (LiDAR) system attached to the vehicle.³⁵

State-space model

The state-space model used in MPC formulation for the path-planning problem is discussed here. The model for the path-planning stage consists of the following double integrator differential equation describing vehicle dynamics as

\overset{\cdot}{x} = v_{x}, {\overset{\cdot}{v}}_{x} = a_{x}, \overset{\cdot}{y} = v_{y}, {\overset{\cdot}{v}}_{y} = a_{y}

(48)

Subsequently the linear discrete time-invariant model is

\begin{matrix} \begin{matrix} X_{p} (k + 1) = A_{d} X_{p} (k) + B_{d} U_{p} (k), \\ Y_{p} (k) = C_{d} X_{p} (k), where \end{matrix} \\ A_{d} = [\begin{matrix} 1 & T & 0 & 0 \\ 0 & 1 & 0 & 0 \\ 0 & 0 & 1 & T \\ 0 & 0 & 0 & 1 \end{matrix}], B_{d} = [\begin{matrix} \frac{T^{2}}{2} & 0 \\ T & 0 \\ 0 & \frac{T^{2}}{2} \\ 0 & T \end{matrix}], C_{d} = I_{4} \end{matrix}

(49)

$A_{d} \in ℝ^{4 \times 4}, B_{d} \in ℝ^{4 \times 2}$ and $C_{d} \in ℝ^{4 \times 4}$ are obtained using the ZOH discretisation method, with integration step T.

Collision avoidance is ensured by incorporating integer constraints, and minimising the objective function given by

\begin{matrix} J (X_{p}, U) = \sum_{j = 1}^{N} [X_{p} (k + j | k) - \bar{X_{p}}] T Q [X_{p} (k + j | k) - \bar{X_{p}}] \\ + \sum_{j = 1}^{M} [Δ U_{p} (k + j - 1 | k)] T R [Δ U_{p} (k + j - 1 | k)] \end{matrix}

(50)

Overprediction and control horizon of length N and M. $Q \in ℝ^{m \times m}$ and $R \in ℝ^{p \times p}$ are symmetrical weighting matrices, with $Q \geq 0$ and R > 0. The discrete-time state space model of equation (49) can be rewritten in an incremental form as

\begin{matrix} ξ (k) = [\begin{matrix} Δ X_{p} (k) \\ Y_{p} (k) \end{matrix}]; \\ ξ (k + 1) = \tilde{A} ξ (k) + \tilde{B} Δ U (k); \\ Y (k) = \tilde{C} ξ (k) \end{matrix}

(51)

The incremental state matrices are defined as

\begin{matrix} \begin{matrix} \tilde{A} = [\begin{matrix} A_{d} & 0_{4 \times 4} \\ C_{d} A_{d} & I_{4} \end{matrix}] \tilde{B} = [\begin{matrix} B_{d} \\ C_{d} B_{d} \end{matrix}] \tilde{C} = [\begin{matrix} 0_{4 \times 4} \\ I_{4} \end{matrix}] T \end{matrix} \end{matrix}

(52) where

Δ X_{p} (k) = X_{p} (k) - X_{p} (k - 1)

is incremental state vector,

Δ U = U (k) - U (k - 1)

is the control input vector, I and

0

are identity and zero matrices of appropriate sizes.

Once the optimal solution is calculated, the set of waypoints serves as the reference trajectory ${\bar{r}}_{t}$ for tracking controller.

Prediction model

The prediction model following the incremental state-space formulation,³⁶ can be obtained as

X_{N} = G Δ U_{M} + F

(53) where

X_{N} \in ℝ^{4 N \times 1}

stacks the predicted values of the state vector along N,

Δ U_{M} \in ℝ^{2 M \times 1}

stacks the incremental control inputs along M,

G \begin{matrix} Δ = [\begin{matrix} \tilde{C} \tilde{B} & 0_{4 \times 2} & \dots & 0_{4 \times 2} \\ \tilde{C} \tilde{A} \tilde{B} & \tilde{C} \tilde{B} & \dots & 0_{4 \times 2} \\ : & : & : \\ \tilde{C} {\tilde{A}}^{M - 1} \tilde{B} & \tilde{C} {\tilde{A}}^{M - 2} \tilde{B} & \dots & \tilde{C} \tilde{B} \\ : & : & : \\ \tilde{C} {\tilde{A}}^{N - 1} \tilde{B} & \tilde{C} {\tilde{A}}^{N - 2} \tilde{B} & \dots & \tilde{C} {\tilde{A}}^{N - M} \tilde{B} \end{matrix}] \in ℝ^{4 N \times 2 M} F \frac{Δ}{= [\begin{matrix} \tilde{C} \tilde{A} \\ \tilde{C} {\tilde{A}}^{2} \\ : \\ \tilde{C} {\tilde{A}}^{N} \end{matrix}] ξ (k) \in R^{4 N}} \end{matrix}

Road constraints

The road constraint for the vehicle position $r_{p}$ is defined using $r_{min}, r_{max} \in R^{2}$ , which depict the minimum and maximum values of the x–y coordinates of the road via the following linear convex inequality constraint

r_{min} \leq r_{p} \leq r_{max}

(54)

Repeating equation (54) along the prediction horizon N, yields

[r_{min}] N \leq X_{N} \leq [r_{max}] N

(55) which can be rewritten in terms of

Δ U_{M}

using equation (53), providing

A Δ U_{M} \leq γ

(56) where

\frac{A Δ}{= [\frac{\begin{matrix} - G \\ G \end{matrix}] \in ℝ^{4 N \times 2 M}; and γ Δ}{= [\begin{matrix} {[r_{max}]}_{N} - F \\ F - {[r_{min}]}_{N} \end{matrix}] \in R^{4 N}}}

Note that in this study only straight roads are considered to formulate the road boundary constraints. The model responses are evaluated for vehicle running on a three-lane straight road though the same formulation can be used to model more complex scenarios of curved roads too with appropriate modifications for road geometry, curvature, etc.

Collision avoidance constraints

The inequality convex constraints for the distances between the autonomous vehicle and the HDV in matrix form can be written as (recall equation (46)),

r_{p} - r^{(j)} \geq Δ

(57)

The vector $Δ \in ℝ^{2}$ corresponds to the error boundaries $Δ_{x}, Δ_{y}$ . Repeating equation (57) along the prediction horizon N yields

X_{N} - [r^{(j)}] N \geq [Δ] N

(58) where

[r^{(j)}] N \in ℝ^{2 N \times 1}

stacks the value of the surrounding vehicle position along the prediction horizon N.

Replacing the prediction model of equation (53) into equation (58), and rewriting in terms of the optimisation vector $X \in ℝ^{(2 M + n_{d} N)}$ , with n_d representing the number of binary variables, one can obtain

T X \leq ϱ

(59) where

\frac{X Δ}{\begin{matrix} = [\frac{\begin{matrix} Δ U_{M} \\ b_{i} \end{matrix}]; T Δ}{= [\begin{matrix} - G & - M I_{\frac{n_{d}}{2} N} & 0_{\frac{n_{d}}{2} N \times \frac{n_{d}}{2} N} \\ G & 0_{\frac{n_{d}}{2} N \times \frac{n_{d}}{2} N} & - M I_{\frac{n_{d}}{2} N]} \end{matrix}} \end{matrix}}

with

T \in ℝ^{n_{d} N \times (2 M + n_{d} N)}

and the vector ϱ is expressed by

\frac{ϱ Δ}{= [\begin{matrix} - {[Δ]}_{N} + F - {[r^{(j)}]}_{N} \\ - {[Δ]}_{N} - F + {[r^{(j)}]}_{N} \end{matrix}] \in ℝ^{n_{d} N}}

Guaranteeing activation of at least one constraint of (47) by

[\begin{matrix} 0_{N \times 2 M} & [1_{n_{d} N}] T \end{matrix}] X \leq [4] N

Path-planning controller

In this work, both path-planner and tracking controllers obtain optimal control vector $U^{*} (k)$ by minimising cost function of the form in equation (50). Rewritten using notation matrix as

{min}_{X} J (k) = \frac{1}{2} X^{T} H X + M^{T} X

(60) where the optimisation vector

\frac{X Δ}{= [Δ U_{M} b_{i}] T}

, is augmented by additional binary decision variables b_i.

The matrix $H$ and the vector ϑ define the terms of the cost function as follows

\frac{1}{2} H = G^{T} Q G + r, M^{T} = 2 (F - [{\bar{r}}_{p}^{(i)}] N) T Q G

(61) where the controlled output weighting matrix is

Q = η \times I_{4 N}

and the control input weighting matrix is

R = ρ \times I_{2 M}

The optimal control vector $U^{*} (k)$ computed at the discrete-time instant k is given by $U^{*} (k) = Δ U^{*} (k) + U^{*} (k - 1)$ , where $Δ U^{*} (k)$ is the first control vector of $Δ U_{M}^{*}$ , which in turn is obtained by solving the optimisation problem in (61) subject to the constraints

S X \leq ϑ

(62)

The matrix $S$ and the vector ϑ define the linear inequality constraint set on the optimisation variables

\frac{S Δ}{= [\frac{\begin{matrix} A \\ T \end{matrix}] \in ℝ^{(n_{d} + 4) N \times (2 M + (n_{d}) N)}, ϑ Δ}{= [\begin{matrix} γ \\ ϱ \end{matrix}] \in ℝ^{(n_{d} + 4) N}}}

Tracking controller

An adequate mathematical model of the system is an important step in designing a control system. A plant cannot be stabilised if the model description is inadequate.

The tracking controller needs an accurate description of vehicle dynamic characteristics in order to control the vehicle behavior. In the present work, we have used a linear MPC to design the tracking controller. The controller performance is evaluated for two different plant models, namely, the bicycle model and the 12-dof model.

Tracking references

As discussed in previous section, the outputs of both the linear vehicle models are $Y = {\begin{matrix} x & y & v_{x} & v_{y} & Ω \end{matrix}}^{T}$ . In order to map these output variables a set of reference inputs have to be assigned to the tracking controller $Y^{ref}$ . The first four reference inputs namely the longitudinal and lateral positions and velocities are obtained from the path-planning MPC as shown in Figure 1. The last input is the reference yaw rate $Ω^{ref}$ , which is obtained using a steady-state yaw rate command.³⁷ Though a dynamic scenario is considered, yet the steady-state (instantaneous) yaw rate is useful in giving an indication of vehicle handling behaviour.³⁸ It is a function of the longitudinal velocity v_x, vehicle characteristic speed V_ch, steering angle δ and wheelbase l and is given by

Ω^{ref} = Ω^{ss} = \frac{v_{x} δ}{l (1 + (\frac{v_{x}}{V_{ch}}) 2)}; V_{ch} = \sqrt{\frac{gl}{K_{us}}}

(63)

In equation (63), K_us is the understeering gradient. The value of K_us for the vehicle under study is 0.95 °/g.

Cost function

The objective function to be minimised to ensure the vehicle follows desired trajectories is the same as given in equation (50) with the difference that the states here are $X \in ℝ^{6}$ and $R^{22}$ for the bicycle and 12-dof model respectively instead of $X_{p} \in ℝ^{4}$ and $U = {T δ_{sw}} T$ instead of $U_{p} = {a_{x}^{p} a_{y}^{p}} T$ .

Prediction model

The prediction model remains the same as given in equation (53) with appropriate change in dimensions of system state matrices of vehicle models instead of path-planner.

Control constraints

The control constraints are incorporated in the scheme and are written in terms of $U$ as

U_{min} \leq U \leq U_{max}, U_{min} = [{\underline{δ}}_{sw} \underline{T}] T, U_{max} = [{\bar{δ}}_{sw} \bar{T}] T

(64) where

U_{min}

and

U_{max}

are the bounds on the control signals. Rewriting the constraint (64) in terms of

Δ U

we obtain

U (k) = [U (k - 1)]_{N} + T_{N} Δ U (k)

(65) where

T_{N} = [\begin{matrix} I_{2} & 0_{2 \times 2} & \dots & 0_{2 \times 2} \\ I_{2} & I_{2} & \dots & 0_{2 \times 2} \\ : & : & \dots & : \\ I_{2} & I_{2} & \dots & I_{2} \end{matrix}] \in ℝ^{2 N \times 2 M}

From equations (65) and (64) we obtain

[U_{min}]_{N} \leq [U (k - 1)]_{N} + T_{N} Δ U (k) \leq [U_{max}]_{N}

(66)

Finally, equation (66) can be written in terms of $Δ U_{M}$ as

Ξ Δ U_{M} \leq Φ

(67) where

Ξ \in ℝ^{4 N \times 2 M}

and

Φ \in ℝ^{4 N}

are given by

\frac{Ξ Δ}{= [\begin{matrix} T_{N} \\ - T_{N} \end{matrix}], Φ = [\begin{matrix} [U_{max}] N - [U (k - 1)] N \\ [U (k - 1)] N - [U_{min}] N \end{matrix}]}

Model predictive controller

Finally, we obtain an optimal control vector $U^{*} (k)$ by minimising the cost function in equation (50) subjected to the additional control constraints but without considering mixed-integer collision avoidance constraints

{min}_{Δ U} J (k) = \frac{1}{2} Δ U^{T} H Δ U + M^{T} Δ U

(68) subject to the control constraints in equation (67).

The objective function of equation (68) is minimised using Matlab's built-in function quadprog to obtain the optimisation vector $Δ U_{M}$ for the tracking controller. For path-planning, the mixed-integer quadratic programing problem needs to be solved in order to take care of the collision avoidance constraints. For this purpose, cplexmiqp solver from IBM³⁹ is used in Matlab to calculate the optimisation vector.

MPC and control constraint parameters are given in Table 2. The sampling time of 0.05 s is chosen in view of real-time implementation capabilities of the proposed controller. A large prediction horizon N and a small control horizon M was chosen for less aggressive control action. Moreover, small M results in computation of fewer variables in the QP solved at each control interval, thereby promoting faster computations.⁴⁰ M < N is essential otherwise some manipulated variable moves might not affect any of outputs and small M promotes an internally stable controller. As a rule of thumb M and N should be chosen as

5 \leq M \leq 20

and

N 3 < M < N 2

.⁴¹ The optimisation was performed in MATLAB using the cplex solver from IBM, which can handle binary variables to solve the MIQP problem. For the 12-dof model, the optimisation is computed in 150 ms to 600 ms using an Intel Core i7-6700U CPU clocked at 3.40GHz with 16GB of RAM. The simulation time increases to around 600 ms when the vehicle approaches the obstacle as the controller has to compute steering and torque control commands. Once the vehicle is past the obstacle the simulation time decreases drastically. The code execution time is faster for the bicycle model and it varies from 100 ms to 500 ms. It is evident that the 12-dof model is fast enough for real-time application.

Table 2.

Parameters of the MPC path-planner.

Variables	Values
Prediction horizon:	N = 40
Control horizon:	M = 5
Sampling time:	$Δ T = 0.05$ s
Control output weights:	$η = [11111] T$
Control input weights:	$ρ = [2015] T$
Control constraints:	${\underline{δ}}_{sw} = - 630 \circ, \underline{T} = 0$ N m
	${\bar{δ}}_{sw} = 630 \circ, \bar{T} = 400$ N m

The actuator constraints are set according to the saturation limits. An equal weight is given to all control outputs η in order to track all vehicle dynamics characteristics ( $X, Y, V_{x}, V - y$ and Ω) with an equal precision. A higher value of weight is given to the steering angle as opposed to the vehicle torque in the input control weighing ρ. This is because of the fact that naturally a driver gives more emphasis in steering the vehicle $δ_{sw}$ than braking (torque T) while avoiding the obstacle. The steering wheel turns three and half turns from one end to the other correspondingly the lower and upper bounds of the steering angle are calculated as $\pm 630 \circ$ . The peak driveline torque is 400 N m.

Results

The scenario description for the cooperative obstacle avoidance manoeuvre is shown in Figure 12(a). The vehicles are sharing plans with one another. Vehicle 1 is in the first lane ( $x = 0, y = 0$ ) and vehicle 2 is in the second lane ( $x = 0, y = 4$ ). The obstacle is at a distance of 200 m and initial velocity of both vehicles is 60 km/h.

Figure 12.

(a) Scenario description: double lane change event; (b) steering wheel angle; (c) engine torque.

The co-simulation study was performed using the path-planning and tracking controller model and the nonlinear complex vehicle dynamics model built in CarSim. Both the vehicles are able to negotiate the obstacle in a cooperative manner. The results are discussed below.

The control inputs (steering $δ_{sw}$ and torque T) are shown in Figure 12(b) and (c). Vehicle 1 steers to the left as it first encounters the obstacle. Once it starts to steers in lane 2, vehicle 2 starts to steer in the third lane to make way for the first vehicle (see the inset 1 in Figure 12(b)). After vehicle 1 has negotiated the obstacle, it returns back to its lane and requires a larger steering input then vehicle 2. It is because of the fact that vehicle 2 started changing its lane after vehicle 1, so it has to take less corrective action to return back to its original lane (see 2 in Figure 12(b)).

Initially, both vehicles start their motion with the same torque. This is because the vehicle has to overcome the rolling resistance and aerodynamic drag which is roughly equal to 125 N for the vehicle speed of 60 km/h. This force has to be counteracted by an engine torque of 40 N m in order to maintain a constant speed of 60 km/h. Vehicle 1 brakes more aggressively as it sees the obstacle first as a result the torque decreases. Once it negotiates the obstacle by turning towards the left, the torque increases again to avoid the obstacle quickly. Eventually it comes back in the same lane and the torque again becomes constant, i.e. 40 N-m (torque required to maintain the same reference preset speed of 60 km/h). Vehicle 2 on other hand just swivels out on lane 3 and its torque increases to avoid colliding vehicle 1.

The trace of vehicle trajectory obtained from the animation interface of CarSim cosimulation is shown in Figure 13. The tracking controller responses are shown in Figure 14. The model outputs (shown using solid lines) are compared against the references (shown with broken lines) generated by the path-planner and steady-state yaw rate reference.

Figure 13.

Cooperative double lane change event in CarSim.

Figure 14.

Comparison of model outputs against the reference inputs for the cooperative path planning and tracking controller: (a) Longitudinal versus lateral position; (b) Longitudinal velocity; (c) Lateral velocity; (d) Yaw rate.

The vehicle path x – y trajectories are shown in Figure 14(a). Vehicle 1 starts in lane 1 and vehicle 2 starts in lane 2. Both the vehicles share each other trajectories (x, y coordinates and longitudinal and lateral velocities v_x, v_y). Vehicle 1 can foresee the stationary obstacle in addition to the shared states of vehicle 2. As it encounters the obstacle, it steers to left moves in the second lane and eventually returns back to the same lane. At the same time, vehicle 2 moves in the third lane in order to make way for vehicle 1 and returns back to its lane after the later has negotiated the obstacle. The important thing to note here is that the motion of vehicle 2 is delayed as it takes steering action only when vehicle 1 starts manoeuvring into its lane. Both vehicles are able to track the reference trajectories and a smooth lane change manoeuvre is made. The tracking errors are shown in Figure 15.

Figure 15.

Lateral tracking errors of vehicles 1 and 2.

The vehicle longitudinal velocity is shown in Figure 14(b). The velocity of vehicle 1 decreases initially because the torque input decreases as it approaches the obstacle. After it negotiates the obstacle, its velocity increases and returns back to the preset reference value of 60 km/h. The velocity variation for vehicle 2 is not substantial. Both the vehicles are able to track the lateral velocity and yaw rate inputs sufficiently well as shown in Figure 14(c) and (d).

The evolution of other remaining states of the 12-dof model for both vehicle 1 (solid-lines) and vehicle 2 (broken-lines) is shown in Figure 16. The CG displacement z shows a minor variation due to load transfer. As the vehicle encounters the obstacle, it swivels out of the lane towards left. Due to the lateral load transfer, the outer left wheels are compressed and the inner right wheels jack up as seen in wheel jounce responses. As the vehicle rolls out of the turn (anticlockwise direction), the roll angle is positive and a little over 1°. The vehicle pitch also shows a similar trajectory though it value is very small as the vehicle is operating nearly at a constant speed. The rear wheel speeds initially decreases because of reduction in wheel torques as the vehicle approaches obstacle. They regain their initial preset references once the vehicle pasts obstacle.

Figure 16.

Evolution of the remaining vehicle states for the cooperative path planning and tracking controller using 12-dof model.

As suggested by an anonymous reviewer, the lateral acceleration and lateral and vertical wheel forces resulting from load transfers are shown in Figure 17. It gives the reader an idea of the magnitude of forces encountered in a normal on-road driving scenario far away from the boundaries of the dynamic envelope.

Figure 17.

Lateral acceleration, lateral and vertical tyre forces for vehicle 1 undergoing the obstacle avoidance manoeuvre.

Model evaluations

In this section we investigate the performance of our model at different speeds and compare the responses obtained from our model against the bicycle model predictions.

Performance evaluation at different speeds

To evaluate the performance of the 12-dof model for a range of operating speeds we have compared the tracking controller results at 40, 60 and 80 km/h in Figure 18(a), (b) and (c), respectively. For this we used different initial conditions of vehicle velocity v_x for linearing the 12-dof model as $X_{e} = {\begin{matrix} {[0]}_{1 \times 10} & v_{x} & {[0]}_{1 \times 5} & \frac{v_{x}}{r_{w}} & \frac{v_{x}}{r_{w}} & {[0]}_{1 \times 4} \end{matrix}}^{T}$ . A brief discussion of performance evaluation results is given below.

Vehicle longitudinal and lateral positions X – Y.

The vehicle is able to track reference path trajectory sufficiently well in all three cases. A sufficiently smooth double lane change manoeuvre is performed. The tracking is better at low speed and inferior at high speeds but still the match is pretty good. This is obvious because of lag in following the reference path at high speeds because of the second-order actuator dynamics.

Lateral velocity v_y.

The vehicle is able to track the lateral velocity quite well in all the three cases. The lateral velocity decreases as speed increases.

Yaw rate Ω.

The vehicle roughly maps the yaw rate input. The reason for the in all three cases mismatch is because a steady-state yaw rate measure is used as a tracking reference which is slightly less than dynamic yaw rate.

Figure 18.

Tracking controller responses at different speeds: (a) Longitudinal versus lateral position; (b) Lateral velocity; (c) Yaw rate.

Comparison against the bicycle model

The vehicle dynamic responses ( $δ_{sw}, x, y, v_{x}, v_{y}$ and Ω) of the 12-dof model are compared against the bicycle model for vehicle 1 for the above mentioned obstacle avoidance scenario in Figure 19.

Figure 19.

Comparison of model responses of 12 dof model and bicycle model: (a) Steering wheel angle; (b) Lateral versus longitudinal position; (c) Lateral velocity; (d) Yaw rate.

The steering angle input computed by the tracking controller using bicycle model is on the higher side as compared with the 12-dof model (see Figure 19(a)). We have earlier seen in Model comparisons section that for the same steering angle input the dynamic behaviour of the vehicle obtained from the 12-dof model matches closely with the sophisticated CarSim model. This implies that if higher values of steering wheel angle are used in a real vehicle according to bicycle model predictions it will result in high values of other dynamic characteristics, which will eventually result in inaccurate tracking of the reference trajectories.

Regarding path tracking, the bicycle model underpredicts the lateral position as compared to the high fidelity model as shown in Figure 19(b) even though it uses a high value of vehicle steering input. This justifies the need of a model of increased complexity for tracking the vehicle trajectory.

There is a substantial amount of difference in the lateral velocity v_y and yaw rate Ω responses between the two models (see Figure 19(c) and (d)). The reason for this mismatch is the unmodelled lateral dynamics of the bicycle model as opposed to the 12-dof model. For instance, refer to equation (5) and $\overset{ࢳ}{j}$ component of a in equation (17).

Bicycle model, $a_{y} = V \overset{\cdot}{β} + \overset{\cdot}{V} β + Ω V .$

\begin{matrix} 12 - dof : a_{y} = {\overset{\cdot}{v}}_{y} - d_{r} \overset{··}{φ} + 2 d_{p} \overset{\cdot}{θ} Ω + d_{p} θ \overset{\cdot}{Ω} \\ + v_{x} Ω + d_{r} φ Ω^{2} \end{matrix}

The effect of pitch θ and roll φ angle is evident on the value of lateral acceleration a_y, which is missing in the bicycle model but captured in the 12-dof model. This shows the superiority of the 12-dof model in tracking the reference trajectories generated by the path-planning MPC.

Conclusion and future work

This work gives a useful insight by evaluating the controller performance for different models. The high fidelity nonlinear vehicle model of intermediate complexity inclusive of roll, pitch and suspension jounce motions is developed. In addition to this, two simplified models namely the kinematic model and the bicycle model were also developed. The models have been compared for a single lane change manoeuvre and it has been established that the 12-dof model emulates the real vehicle dynamic characteristics fairly well. This nonlinear model is subsequently linearised to take an account of different operating conditions.

In the next part, the paper addressed the problem of path-planning and tracking controller for an autonomous vehicle considering the trajectories of other autonomous vehicle. The problem is solved using a hierarchical MPC design with MIQP at the planning layer. Integer constraints are incorporated to ensure collision avoidance between the autonomous vehicles and stationary obstacle. Control actuation constraints have also been incorporated in the proposed approach. To evaluate the effectiveness of the method, numerical simulations are performed for a cooperative double lane change manoeuvre, which demonstrate the effectiveness of the proposed method. The tracking controller is able to follow the references and the method is quite appropriate to attain the on-road autonomous driving environment. This is because the linear tracking controller uses only 22 state variables to control the high fidelity nonlinear vehicle model in CarSim.

In future, the current method with linear MPC will be compared with nonlinear MPC and other alternatives will be evaluated to account for probabilistic trajectories and soft constraints. An initial investigation of these concepts has been evaluated by the Viana et al.⁴² Moreover the goal is to demonstrate cooperative autonomy in an urban environment where the obstacles are moving human-driven vehicles as opposed to stationary obstacles. This work will be extended to incorporate moving obstacles with probabilistic trajectories coming from the human driver model.⁴³

Finally, this algorithm is planned to be implemented in a real-time vehicle operation and that can be taken up as the future developmental work.

Footnotes

Acknowledgements

This project comprises a consortium with six partners, namely, Applus IDIADA, Cosworth Electronics, Connected Places Catapult, Westfield Sportscars, SBD Automotive and Cranfield University. This work is planned to be implemented in real vehicles by the end of the project.

Declaration of Conflicting Interests

The author(s) declared no potential conflicts of interest with respect to the research, authorship, and/or publication of this article.

Funding

The author(s) disclosed receipt of the following financial support for the research, authorship, and/or publication of this article: This work has been supported by Innovate UK within the scope of the project grant MuCCA ‘Multi-Car Collision Avoidance’.

References

Wang

, et al. Model predictive control-based cooperative lane change strategy for improving traffic flow. Adv Mech Eng 2016; 8: 1687814016632992.

Hassanzadeh M, Lidberg M, Keshavarz M, et al. Path and speed control of a heavy vehicle for collision avoidance manoeuvres. In: 2012 IEEE intelligent vehicles symposium, 2012, pp.129–134. New York: IEEE.

Qian X, Altché F, Bender P, et al. Optimal trajectory planning for autonomous driving integrating logical constraints: an MIQP perspective. In: 2016 IEEE 19th international conference on intelligent transportation systems (ITSC), 2016, pp.205–210. New York: IEEE.

Shim

Adireddy

Yuan

. Autonomous vehicle collision avoidance system using path planning and model-predictive-control-based active front steering and wheel torque control. Proc IMechE, Part D: J Automobile Engineering 2012; 226: 767–778.

Suzuki

Usami

Maekawa

. Automatic two-lane path generation for autonomous vehicles using quartic b-spline curves. IEEE Trans Intell Veh 2018; 3: 547–558.

Khatib M, Jaouni H, Chatila R, et al. Dynamic path modification for car-like nonholonomic mobile robots. In: Proceedings of international conference on robotics and automation, vol. 4, 1997, pp.2920–2925. New York: IEEE.

Khajepour

Melek

, et al. Path planning and tracking for vehicle collision avoidance based on model predictive control with multiconstraints. IEEE Trans Veh Technol 2017; 66: 952–964.

Schouwenaars T, De Moor B, Feron E, et al. Mixed integer programming for multi-vehicle path planning. In: 2001 European control conference (ECC), 2001, pp.2603–2608. New York: IEEE.

Frese C and Beyerer J. A comparison of motion planning algorithms for cooperative collision avoidance of multiple cognitive automobiles. In: 2011 IEEE intelligent vehicles symposium (IV), 2011, pp.1156–1162. New York: IEEE.

10.

Rovira-Más

Zhang

. Fuzzy logic control of an electrohydraulic valve for auto-steering off-road vehicles. Proc IMechE, Part D: J Automobile Engineering 2008; 222: 917–934.

11.

Leonard

How

Teller

, et al. A perception-driven autonomous urban vehicle. J Field Robot 2008; 25: 727–774.

12.

Urmson

Ragusa

Ray

, et al. A robust approach to high-speed navigation for unrehearsed desert terrain. J Field Robot 2006; 23: 467–508.

13.

Levinson J, Askeland J, Becker J, et al. Towards fully autonomous driving: Systems and algorithms. In: 2011 IEEE intelligent vehicles symposium (IV), 2011, pp.163–168. New York: IEEE.

14.

Tabatabaei Oreh

Kazemi

Azadi

. A sliding-mode controller for directional control of articulated heavy vehicles. Proc IMechE, Part D: J Automobile Engineering 2014; 228: 245–262.

15.

Nam

Fujimoto

, et al. Robust yaw stability control for electric vehicles based on active front steering control through a steer-by-wire system. Int J Autom Technol 2012; 13: 1169–1176.

16.

Kanchwala H and Bordons C. Improving handling performance of an electric vehicle using model predictive control. SAE Technical Paper 2015-01-0082, 2015.

17.

Kanchwala H, Viana ÍB, Ceccoti M, et al. Model predictive tracking controller for a high fidelity vehicle dynamics model. In: 2019 IEEE intelligent transportation systems conference (ITSC), 2019, pp.1496–1503. New York: IEEE.

18.

Kong J, Pfeiffer M, Schildbach G, et al. Kinematic and dynamic vehicle models for autonomous driving control design. In: 2015 IEEE intelligent vehicles symposium (IV), 2015, pp.1094–1099. New York: IEEE.

19.

Tomas-Gabarron

Egea-Lopez

Garcia-Haro

. Vehicular trajectory optimization for cooperative collision avoidance at high speeds. IEEE Trans Intell Transp Syst 2013; 14: 1930–1941.

20.

Anwar

. Generalized predictive control of yaw dynamics of a hybrid brake-by-wire equipped vehicle. Mechatronics 2005; 15: 1089–1108.

21.

Zhao C, Xiang W and Richardson P. Vehicle lateral control and yaw stability control through differential braking. In: 2006 IEEE international symposium on industrial electronics, vol. 1, pp.384–389. New York: IEEE.

22.

Kanchwala

Wideberg

. Pitch reduction and traction enhancement of an EV by real-time brake biasing and in-wheel motor torque control. Int J Veh Syst Model Test 2016; 11: 165–192.

23.

Polack P, Altché F, d’Andréa Novel B, et al. The kinematic bicycle model: a consistent model for planning feasible trajectories for autonomous vehicles? In: 2017 IEEE intelligent vehicles symposium (IV), 2017, pp.812–818. New York: IEEE.

24.

Gutjahr

Gröll

Werling

. Lateral vehicle trajectory optimization using constrained linear time-varying MPC. IEEE Trans Intell Transp Syst 2016; 18: 1586–1595.

25.

Kanchwala

Ogai

. Development of an intelligent transport system for EV. SAE Int J Passen Cars-Electron Electric Syst 2016; 9: 9–21.

26.

Funke

Brown

Erlien

, et al. Collision avoidance and stabilization for autonomous vehicles in emergency scenarios. IEEE Trans Control Syst Technol 2017; 25: 1204–1216.

27.

Yuan

Sun

Gordon

. Unified decision-making and control for highway collision avoidance using active front steer and individual wheel torque control. Veh Syst Dyn 2019; 57: 1188–1205.

28.

Pacejka

. Tire and vehicle dynamics, New York: Elsevier, 2005.

29.

Abe M. Vehicle active motion controls. In: Abe M (ed.) Vehicle handling dynamics. 2nd ed. Oxford: Butterworth-Heinemann, 2015, pp.201–229.

30.

Kanchwala

Trigell

. Vehicle handling control of an electric vehicle using active torque distribution and rear wheel steering. Int J Veh Des 2017; 74: 319–345.

31.

Bakker E, Nyborg L and Pacejka H. Tyre modelling for use in vehicle dynamics studies. Technical report, SAE Technical Paper, 1987.

32.

Velenis

Frazzoli

Tsiotras

. Steady-state cornering equilibria and stabilisation for a vehicle during extreme operating conditions. Int J Veh Autonom Syst 2010; 8: 217–241.

33.

Westfield. Sports cars. https://westfield-sportscars.co.uk/ (accessed 22 July 2020).

34.

Williams H and Brailsford S. Computational logic and integer programming. Oxford: Oxford University Press, Inc., 1996.

35.

Eckelmann

Trautmann

Ußler

, et al. V2v-communication, lidar system and positioning sensors for future fusion algorithms in connected vehicles. Transp Res Procedia 2017; 27: 69–76.

36.

Maciejowski

. Predictive control: with constraints, London: Pearson Education, 2002.

37.

Wong J. Theory of ground vehicles. New York: John Wiley & Sons, 2008.

38.

Smith E, Tavernini D, Claret C, et al. Optimal yaw-rate target for electric vehicle torque vectoring system. In: The dynamics of vehicles on roads and tracks: proceedings of the 24th symposium of the international association for vehicle system dynamics (IAVSD 2015), Graz, Austria, 17–21 August 2015. Boca Raton, FL: CRC Press.

39.

IBM. IBM ILOG CPLEX V12.1. User manual for CPLEX, 2009.

40.

Zhao

Maxim

Liu

, et al. Effect of control horizon in model predictive control for steam/water loop in large-scale ships. Processes 2018; 6: 265.

41.

Seborg

Mellichamp

Edgar

, et al. Process dynamics and control, New York: John Wiley & Sons, 2010.

42.

Viana I, Kanchwala H and Aouf N. Cooperative trajectory planning for autonomous driving using nonlinear model predictive control. In: 2019 IEEE international conference on connected vehicles and expo (ICCVE) (IEEE ICCVE 2019), Graz, Austria, 2019.

43.

Kanchwala H. Path planning and tracking of an autonomous car with high fidelity vehicle dynamics model and human driver trajectories. In: 2019 IEEE 10th international conference on mechanical and aerospace engineering (ICMAE), 2019, pp.306–313. New York: IEEE.

Cooperative path-planning and tracking controller evaluation using vehicle models of varying complexities

Abstract

Keywords

Introduction

Methodology

Vehicle model

Kinematic model

Bicycle model

12-dof model

Coordinate system

Planar motions

Out-of-plane motions

Model comparisons

Linearisation

Path-planning

Collision avoidance

Remark 1

State-space model

Prediction model

Road constraints

Collision avoidance constraints

Path-planning controller

Tracking controller

Tracking references

Cost function

Prediction model

Control constraints

Model predictive controller

Results

Model evaluations

Performance evaluation at different speeds

Comparison against the bicycle model

Conclusion and future work

Footnotes

Acknowledgements

Declaration of Conflicting Interests

Funding

References