Optimal Predictive Control for Path Following of a Full Drive-by-Wire Vehicle at Varying Speeds

Abstract

The current research of the global chassis control problem for the full drive-by-wire vehicle focuses on the control allocation (CA) of the four-wheel-distributed traction/braking/steering systems. However, the path following performance and the handling stability of the vehicle can be enhanced a step further by automatically adjusting the vehicle speed to the optimal value. The optimal solution for the combined longitudinal and lateral motion control (MC) problem is given. First, a new variable step-size spatial transformation method is proposed and utilized in the prediction model to derive the dynamics of the vehicle with respect to the road, such that the tracking errors can be explicitly obtained over the prediction horizon at varying speeds. Second, a nonlinear model predictive control (NMPC) algorithm is introduced to handle the nonlinear coupling between any two directions of the vehicular planar motion and computes the sequence of the optimal motion states for following the desired path. Third, a hierarchical control structure is proposed to separate the motion controller into a NMPC based path planner and a terminal sliding mode control (TSMC) based path follower. As revealed through off-line simulations, the hierarchical methodology brings nearly 1700% improvement in computational efficiency without loss of control performance. Finally, the control algorithm is verified through a hardware in-the-loop simulation system. Double-lane-change (DLC) test results show that by using the optimal predictive controller, the root-mean-square (RMS) values of the lateral deviations and the orientation errors can be reduced by 41% and 30%, respectively, comparing to those by the optimal preview acceleration (OPA) driver model with the non-preview speed-tracking method. Additionally, the average vehicle speed is increased by 0.26 km/h with the peak sideslip angle suppressed to 1.9°. This research proposes a novel motion controller, which provides the full drive-by-wire vehicle with better lane-keeping and collision-avoidance capabilities during autonomous driving.

1 Introduction

A full drive-by-wire vehicle is an ideal platform to study the next-generation global chassis control problem, whose four-wheel traction/braking torques and steering angles are controlled simultaneously and independently [1]. This kind of over-actuated distributed control system is proven to be able to enhance the vehicle dynamic performance by making the best use of the available tire forces [2]. Based on the control hierarchy summarized by JOHANSEN et al. [3], the vehicle dynamics control algorithm is functionally divided into two: the motion control (MC) and the control allocation (CA) levels. HARKEGARD et al. [4] pointed out that by choosing weight matrices appropriately, the high-level MC and the low-level CA can be separated without loss of control performance in the framework of optimal control. As demonstrated in previous work, the tire force CA level reflects the superiority of the integrated chassis controller for the full drive-by-wire vehicles. This is for the reason that the redundant degrees of control freedom provide additional flexibility in the CA design, so that various objective functions can be formulated to gain some extra performance, for example, maximization of MC efforts [5, 6], minimization of tire workloads [6–9], and increase of the total energy efficiency [10, 11], etc.

In view of the MC design, the combined control of longitudinal and lateral dynamics is often neglected by considering the capabilities of the vehicle more than those of the human driver [12]. Nevertheless, there are still some improvements to be made. CHEN et al. [13] demonstrates that the vehicle stability depends not only on the lateral dynamic states during the steering process but also on the longitudinal states before steering. Although the idea of improving the vehicle lateral stability via limiting the vehicle speed is intuitive and theoretically appealing, yet it is undesirable that the speed drops to such a low level that neither the driver nor the chassis controller is able to make a fast lane change to avoid an oncoming obstacle. Nonetheless, it is possible to develop a motion controller, the control objective of which is to automatically follow a given path as closely as possible while automatically adjusting the vehicle speed to its optimal value. This provides the vehicle with enhanced lane-keeping and collision-avoidance capabilities during autonomous driving. Meanwhile, the vehicle handling stability is guaranteed regardless of whether the tires are operating in their nonlinear region. Moreover, because all the actuators in a full drive-by-wire vehicle are using X-by-wire technology, it is more suitable for achieving autonomous driving than a conventional vehicle [14].

On account of the nonlinear coupling between any two directions of the vehicle motion, the path following problem at varying speeds is often decomposed into a trajectory tracking and a velocity tracking sub-tasks [15–18], which simplifies the optimization problem but also yields a sub-optimal solution as compared to incorporating the longitudinal and lateral control within the same framework. Another option is to employ the model predictive control (MPC) to solve the combined MC problem under the state and input constraints, which is an attractive method for the automated lane guidance [19, 20] and the obstacle avoidance [21] applications. Note that due to the computational complexity of solving the constrained finite-time optimal control (CFTOC) problem, nonlinear MPC (NMPC) is still not generally applicable to agile autonomous vehicles, thus limiting the usability of detailed nonlinear prediction models [14]. In an attempt to overcome this, researchers often reduced the computational effort by means of successive online linearization of the nonlinear vehicle model, which is approximated by a linear time-varying (LTV) system with a parameter-varying state transition and a parameter-varying input matrix [22–25]. However, the performance of the LTV MPC deteriorates when the prediction horizon is large. Besides, their assumption on the constant velocity for obtaining the reference trajectories over the prediction horizon is invalid when the vehicle travels at varying speeds.

Based on the aforementioned consideration, this paper presents a hierarchical motion controller featuring a high level path-follow motion planner and a low level motion follower. The proposed methodology of decomposition of the NMPC problem into a two-level MC problem allows the motion controller to be implemented in real time. The rest of this paper is organized as follows: the spatial vehicle dynamics model is described in section 2; the whole motion controller design is described in section 3, in which the detailed formulation of the CFTOC problem that originates in Ref. [14] is illustrated in section 3.1, the hierarchical structure as well as the calculation of the reference motion states is presented in section 3.2 and the motion follower for path following is introduced in section 3.3; In section 4, the proposed controller with the tire force CA for the full drive-by-wire vehicles as discussed by SONG et al. [6] is validated through both the off-line and real-time simulations by comparing the results with other control configurations; The conclusions are reached in section 5.

2 Spatial Vehicle Dynamics Model

Fig. 1 describes the planar motion of the vehicle body within a lane. The vehicle model undergoes a coordinate transformation to derive the dynamics of the vehicle about the reference trajectory, such that the independent variable becomes the distance along the path s _d instead of time t. This spatial transformation allows the position of the vehicle as well as the tracking errors to be known explicitly at each sampling instant even if the vehicle travels a portion of the desired path at varying speeds. The time-dependent vehicle dynamics can be transformed into the position-dependent dynamics by multiplying the scaling factor S _CF, which is derived from the geometry of Fig. 1:

$$S_{\text{CF}} = (1 - e_{y} /\rho_{\text{d}} )/(V_{x} \cos e_{\psi } - V_{y} \sin e_{\psi } ),$$

(1)

where e _y and e _ψ denote the lateral deviation error and the orientation error at the cog (center of gravity), respectively; ρ _d is the radius of curvature at the sampling point; V _x and V _y are the longitudinal and lateral velocities, respectively; v _s is the projected vehicle speed along the direction of the reference trajectory.

Fig. 1

Vehicle model in a road-aligned coordinate frame

The spatial vehicle dynamics model is described by the following set of differential equations, given by Eq. (2). Here and hereinafter, the superscript of $\left( \cdot \right)^{\prime }$ denotes the spatial derivative with respect to the road.

$$\begin{aligned} & V^{\prime}_{x} = (\dot{\psi }V_{y} - \rho_{0} C_{{\rm D}} A_{{\rm f}} V_{x}^{2} /2m + F_{x{\rm d}} /m) \cdot S_{{{\text{CF}}}} ,\\ & V^{\prime}_{y} = ( - \dot{\psi }V_{x} + F_{{y{\text{d}}}} /m) \cdot S_{{{\text{CF}} }}, \\ & \psi^{\prime\prime} = (M_{{z{\text{d}}}} /I_{z} ) \cdot S_{{{\text{CF}} }} ,\\ & e^{\prime}_{\psi } = \dot{\psi } \cdot S_{\text{CF}} - \psi^{\prime}_{{{\text{d}} }} ,\\ & e^{\prime}_{y} = (\dot{V}_{x} \sin e_{\psi } + \dot{V}_{y} \cos e_{\psi } ) \cdot S_{{{\text{CF}} }} ,\\ \end{aligned}$$

(2)

where ψ is the yaw angle, the first- and second- order time derivatives of which are the yaw rate and yaw angular acceleration, respectively and ψ _d is the road path heading angle; m is the total vehicle mass; I _z is the moment of inertia round the vertical axis; ρ ₀ is the air density; C _D is the drag coefficient factor; A _f is the vehicle front area; F _xd and F _yd are the resultant longitudinal and lateral forces of four tires, respectively and M _zd is the resultant yaw moment.

The complexity of the MC problem is greatly reduced by taking into account as few variables as possible. Therefore, the motion controller only needs to determine F _xd, F _yd, and M _zd for the vehicular planar motion. The range of the control efforts should be properly defined such that the tire forces can be distributed within the tire adhesive limits, as shown in Fig. 2. However, as for a conventional vehicle, the longitudinal and lateral tire forces of the four wheels are internally coupled with respect to the mechanisms like steering linkage, transmission, and differential, etc. The explicit constraint of these efforts is hard to determine and the electronic control systems may not be able to achieve the resultant forces and moment. According to HATTORI et al. [26], the constraint between the resultant forces and moment of the full drive-by-wire vehicle is modeled as the convex set, expressed as

$${{\sqrt {F_{{x{\text{d}}}}^{2} + F_{{y{\text{d}}}}^{2} + W_{\text{r}}^{2} M_{{z{\text{d}}}}^{2} } } \mathord{\left/ {\vphantom {{\sqrt {F_{{x{\text{d}}}}^{2} + F_{{y{\text{d}}}}^{2} + W_{\text{r}}^{2} M_{{z{\text{d}}}}^{2} } } {\left( {mg\mu } \right)}}} \right. \kern-0pt} {\left( {mg\mu } \right)}} \le \lambda_{{{\text{v}},\hbox{max} }} < 1,$$

(3)

where g is the gravitational constant (9.8 m/s²); μ is the friction coefficient; W _r is the constant weight balancing forces and moment, which in Ref. [2] is chosen as the reciprocal of the typical length between the wheel and the cog; the factor λ _v,max is introduced to describe the upper bound of the total workload of the four tires. For a full drive-by-wire vehicle, λ _v,max can be regarded as a constant value during the control interval and is set to 0.95 in this paper.

Fig. 2

Range of the motion control efforts by taking the tire adhesive limits into consideration

The nonlinear vehicle dynamics can be expressed in the following compact form:

$${{\varvec{x}}^{{\prime}}_{\text{c}}} \left( {s_{\text{d}} } \right) = f_{\text{c}}^{{s_{\text{d}} }} \left( {{\varvec{x}}_{\text{c}} \left( {s_{\text{d}} } \right),{\varvec{u}}_{\text{c}} \left( {s_{\text{d}} } \right)} \right),$$

(4)

where the subscript of (·)_c denotes the control-related variables here and hereinafter; the state vector x _c and the input vector u _c are given by:

$$\begin{aligned} {\varvec{x}}_{\text{c}} & = \left( {\begin{array}{*{20}c} {V_{x} } & {V_{y} } & {\dot{\psi }} & {e_{\psi } } & {e_{y} } \\ \end{array} } \right)^{\text{T}} , \\ {\varvec{u}}_{\text{c}} & = \left( {\begin{array}{*{20}c} {F_{{x{\text{d}}}} } & {F_{{y{\text{d}}}} } & {M_{{z{\text{d}}}} } \\ \end{array} } \right)^{\text{T}} . \\ \end{aligned}$$

(5)

By using the Euler method, the spatial vehicle dynamics model is discretized at the sampling point $s_{\text{d}}^{ * }$ with the fixed sampling distance ds _d, given by

$${\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } + {\rm d}s_{\text{d}} } \right) = f_{\text{cd}}^{{s_{\text{d}} }} \left( {{\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } } \right),{\varvec{u}}_{\text{c}} \left( {s_{\text{d}}^{ * } } \right)} \right).$$

(6)

Also, we denote by Δ u _c the control input increment computed at the sampling point $s_{\text{d}}^{ * }$:

$${\rm \Delta} {\varvec{u}}_{\text{c}} \left( {s_{\text{d}}^{ * } } \right) = {\varvec{u}}_{\text{c}} \left( {s_{\text{d}}^{ * } } \right) - {\varvec{u}}_{\text{c}} \left( {t - T_{\text{s}} } \right),$$

(7)

where u _c(t − T _s) is the applied control input at the last sampling time.

3 Controller Design

A NMPC problem is formulated to achieve the optimal combined MC for the full drive-by-wire vehicles, in which the nonlinear prediction model is the above-mentioned spatial vehicle dynamics model. Also, a hierarchical control structure is proposed to improve the system’s real-time performance without sacrificing its tracking ability.

3.1 Spatial Model Predictive Controller

As shown in Fig. 3, the basis of the MPC method is to predict the evolution of the plant model over a finite horizon based on a sequence of future inputs in order to optimize the system cost under the given constraints with restricted preview.

Fig. 3

Schematic of model predictive control

As for the spatial MPC (SMPC), the optimization variables are chosen as the state sequence ξ _c and the input increments vector Δ U _c with respect to the sampling points along the reference trajectory, given by Eqs. (8) and (9), respectively.

$${\boldsymbol{\xi}}_{\text{c}} \left( {s_{\text{d}}^{ * } } \right) = \left( {\begin{array}{*{20}c} {{\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } + {\rm d}s_{\text{d}} } \right)} \\ {{\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } + 2 \cdot {\rm d}s_{\text{d}} } \right)} \\ \vdots \\ {{\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } + H_{\text{P}} \cdot {\rm d}s_{\text{d}} } \right)} \\ \end{array} } \right)^{\text{T}} ,$$

(8)

$${\rm \Delta} {\varvec{U}}_{\text{c}} \left( {s_{\text{d}}^{ * } } \right) = \left( {\begin{array}{*{20}c} {{\rm \Delta} {\varvec{u}}_{\text{c}} \left( {s_{\text{d}}^{ * } } \right)} \\ {{\rm \Delta} {\varvec{u}}_{\text{c}} \left( {s_{\text{d}}^{ * } + {\rm d}s_{\text{d}} } \right)} \\ \vdots \\ {{\rm \Delta} {\varvec{u}}_{\text{c}} \left( {s_{\text{d}}^{ * } + \left( {H_{\text{C}} - 1} \right) \cdot {\rm d}s_{\text{d}} } \right)} \\ \end{array} } \right)^{\text{T}} ,$$

(9)

where H _P is the length of the prediction horizon and H _C is the length of the control horizon.

The cost function J _c to be minimized takes the following form:

$$\begin{aligned} J_{\text{c}} \left( {{\boldsymbol{\xi}}_{\text{c}} \left( {s_{\text{d}}^{ * } } \right),{\rm \Delta} {\varvec{U}}_{\text{c}} \left( {s_{\text{d}}^{ * } } \right)} \right) \\ \quad = \sum\limits_{i = 1}^{{H_{\text{P}} }} {\left\| {{\varvec{C}}_{\text{c}} {\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } + i \cdot {\rm d}s_{\text{d}} } \right) - {\varvec{y}}_{\text{ref}} \left( {s_{\text{d}}^{ * } + i \cdot {\rm d}s_{\text{d}} } \right)} \right\|_{{{\boldsymbol{Q}}_{\text{c}} }}^{2} } \\ \quad \quad + \sum\limits_{i = 1}^{{H_{\text{C}} }} {\left\| {{\varvec{u}}_{\text{c}} \left( {s_{\text{d}}^{ * } + \left( {i - 1} \right) \cdot {\rm d}s_{\text{d}} } \right)} \right\|_{{{\boldsymbol{R}}_{\text{c}} }}^{2} } \\ \quad \quad + \sum\limits_{i = 1}^{{H_{\text{C}} }} {\left\| {{\rm \Delta} {\varvec{u}}_{\text{c}} \left( {s_{\text{d}}^{ * } + \left( {i - 1} \right) \cdot {\rm d}s_{\text{d}} } \right)} \right\|_{{{\varvec{S}}_{\text{c}} }}^{2} } , \\ \end{aligned}$$

(10)

where Q _c, R _c, and S _c are 3 × 3 weighting matrices to penalize the tracking errors, the control input efforts and the control input increments, respectively;

• ${\varvec{C}}_{\text{c}}$—system output matrix,

• ${\varvec{C}}_{\text{c}} = \left( {\begin{array}{*{20}c} 1 & 0 & 0 & 0 & 0 \\ 0 & 0 & 0 & 1 & 0 \\ 0 & 0 & 0 & 0 & 1 \\ \end{array} } \right);$

• ${\varvec{y}}_{\text{ref}}$—reference output vector,

• ${\varvec{y}}_{\text{ref}} = \left( {\begin{array}{*{20}c} {v_{\text{ref}} } & 0 & 0 \\ \end{array} } \right)^{\text{T}}$;

• $v_{\text{ref}}$—longitudinal reference speed.

The vehicle speed becomes critical when approaching a bend. Since no active lateral stabilization is considered in the cost function J _c, the vehicle stability may be lost and/or the tracking performance will be degraded in extreme handling maneuvers at high speed. Therefore, the longitudinal reference velocity v _ref should be adapted. According to ATTIA et al. [18], the maximum longitudinal velocity v _lim considering the road curvature is given by:

$$v_{\lim } = \lambda_{\text{d}} \sqrt {{{g\mu } \mathord{\left/ {\vphantom {{g\mu } {\rho_{\text{d}} }}} \right. \kern-0pt} {\rho_{\text{d}} }}} ,$$

(11)

where the adjustment factor λ _d is employed to adjust the speed limit and is set to 0.85 in this paper.

It can be seen that v _lim only depends on the road geometry information and can be readily evaluated in real time. Nevertheless, the simple description given by this criterion may be inappropriate to determine the admissible speed. For example, the maximum speed on a straight lane is infinite. Therefore, the longitudinal reference velocity is defined by

$$v_{\text{ref}} = \hbox{min} \left( {v_{\text{set}} ,v_{\lim } } \right),$$

(12)

where v _set is the speed profile considering the legal speed limit.

In summary, assuming that the full measurement or estimation of the vehicle state x _c is available at the current running time t, the SMPC problem is then formulated as follow, which is repeatedly solved online for a finite receding horizon:

$$\begin{aligned} \mathop {\hbox{min} }\limits_{{{\boldsymbol{\xi}}_{\text{c}}^{{s_{\text{d}} }} ,{\rm \Delta} {\varvec{U}}_{\text{c}}^{{s_{\text{d}} }} }} \, J_{\text{c}} \left( {{\boldsymbol{\xi}}_{\text{c}} \left( {s_{\text{d}}^{ * } } \right),{\rm \Delta} {\varvec{U}}_{\text{c}} \left( {s_{\text{d}}^{ * } } \right)} \right) \\ {\text{s}} . {\text{t}} . { } {\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } + \left( {i + 1} \right) \cdot {\rm d}s_{\text{d}} } \right) = f_{\text{cd}}^{{s_{\text{d}} }} \left( {{\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } + i \cdot {\rm d}s_{\text{d}} } \right),} \right. \left. {{\varvec{u}}_{\text{c}} \left( {s_{\text{d}}^{ * } + i \cdot {\rm d}s_{\text{d}} } \right)} \right), \\ \quad \forall i = 0, \ldots ,H_{\text{P}} - 1, \\ {\varvec{u}}_{\text{c}} \left( {s_{\text{d}}^{ * } + i \cdot {\rm d}s_{\text{d}} } \right) = {\varvec{u}}_{\text{c}} \left( {s_{\text{d}}^{ * } + \left( {i - 1} \right) \cdot {\rm d}s_{\text{d}} } \right) + {\rm \Delta} {\varvec{u}}_{\text{c}} \left( {s_{\text{d}}^{ * } + i \cdot {\rm d}s_{\text{d}} } \right),\\ \quad \forall i = 0, \ldots ,H_{\text{P}} - 1, \\ {\rm \Delta} {\varvec{u}}_{\text{c}} \left( {s_{\text{d}}^{ * } + i \cdot {\rm d}s_{\text{d}} } \right) = 0,\quad \forall i = H_{\text{C}} , \ldots ,H_{\text{P}} - 1, \\ \left\| {{\varvec{u}}_{\text{c}} \left( {s_{\text{d}}^{ * } + i \cdot {\rm d}s_{\text{d}} } \right)} \right\|_{{\varvec{W}}}^{2} \le \lambda_{\text{v,max}} mg\mu ,\quad \forall i = 0,1, \ldots ,H_{\text{C}} - 1, \\ {\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } } \right) = {\varvec{x}}_{\text{c}} \left( t \right),\quad \forall t \ge 0, \\ {\varvec{u}}_{\text{c}} \left( {s_{\text{d}}^{ * } - {\rm d}s_{\text{d}} } \right) = {\varvec{u}}_{\text{c}} \left( {t - T_{\text{s}} } \right),\quad \forall t \ge T_{\text{s}} , \\ \end{aligned}$$

(13)

where ${\varvec{W}} = {\text{diag}}\left( {\begin{array}{*{20}c} 1 & 1 & {W_{\text{r}} } \\ \end{array} } \right).$

The nonlinear CFTOC problem is calculated using an interior-point algorithm. Parameters H _P, H _C, R _c, S _c, and Q _c determine the performance of the SMPC feedback control and should be finely tuned to guarantee stability and feasibility [27]. It is noted that active collision avoidance can be easily realized with the proposed controller by simply imposing the obstacle constraints on the vehicle position states in this formulation.

Let $\Delta {\varvec{U}}_{\text{c,1}}^{{s_{\text{d}} }}$ denote the first sample of the optimal solution $\Delta {\varvec{U}}_{\text{c}}$ at the sampling point $s_{\text{d}}^{ * } .$ The closed-loop control law applied to the system at the current running time t is given by:

$${\varvec{u}}_{\text{c}} \left( t \right) = {\varvec{u}}_{\text{c}} \left( {t - T_{\text{s}} } \right) + {\rm \Delta} {\varvec{U}}_{\text{c,1}}^{{s_{\text{d}} }} .$$

(14)

At the next sampling instant, the problem is solved over the shifted horizon based on the new state. The spatial step size $ds_{\text{d}}$ should be adapted so that the calculated state ${\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } + ds_{\text{d}} } \right)$ at the next distance point corresponds to the system’s actual state ${\varvec{x}}_{\text{c}} \left( {t + T_{\text{s}} } \right)$ at the next time step. Only then can the generated control efforts correspond to the control actions applied to the plant in the time domain. The spatial step size at point $s_{\text{d}}^{ * }$ is given by

$$\begin{aligned} {\rm d}s_{\text{d}}^{{s_{\text{d}}^{*} }} & = \left( {V_{x} \left( {s_{\text{d}}^{*} } \right) \cdot \cos \left( {e_{\psi } \left( {s_{\text{d}}^{*} } \right)} \right) - V_{y} \left( {s_{\text{d}}^{*} } \right) \cdot \sin \left( {e_{\psi } \left( {s_{\text{d}}^{*} } \right)} \right)} \right) \\ & \quad \times \left( {1 - {{e_{y} \left( {s_{\text{d}}^{*} } \right)} \mathord{\left/ {\vphantom {{e_{y} \left( {s_{\text{d}}^{*} } \right)} {\rho \left( {s_{\text{d}}^{*} } \right)}}} \right. \kern-0pt} {\rho \left( {s_{\text{d}}^{*} } \right)}}} \right)^{ - 1} \times T_{\text{s}} . \\ \end{aligned}$$

(15)

The SMPC controller only calculates the resultant forces and moment of the vehicle in order to best follow the given trajectory. These MC efforts are optimally distributed to the four wheels under some performance index or cost function. At last, the magnitude and direction of each tire force should be individually translated to the steering angle and traction/braking torque of each wheel.

3.2 Hierarchical Control Scheme

At each iteration step, the SMPC controller developed in the above section needs to obtain an exact optimal solution for the high-dimensional differential equations of the spatial vehicle dynamics model with strong nonlinearity under the given constraints (i.e. the ellipsoidal constraint of the MC efforts). As a result, simulations are very time consuming and the tuning is difficult. However, only the first sample of the optimal sequence of the MC efforts is applied to the system via the control allocation layer and the executive layer, which in some way causes unnecessary computational cost. In order to improve the real-time performance, this paper proposes a hierarchical control structure by implementing the SMPC controller for path-follow motion planning and introducing the motion follow layer to calculate the resultant forces and moment of the vehicle, as shown in Fig. 4.

Fig. 4

Real-time performance improvement via hierarchical control structure

Let ${\boldsymbol{\xi}}_{\text{c}}^{{s_{\text{d}} }}$ denote the sequence of optimal motion states over the prediction horizon H _P at the sampling point $s_{\text{d}}^{ * }$, which is generated by SMPC with lower frequency, given by

$${\boldsymbol{\xi}}_{\text{c}}^{{s_{\text{d}} }} = \left( {{\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } + {\rm d}s_{\text{d}}^{{s_{\text{d}}^{*} }} } \right), \ldots ,{\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } + H_{\text{P}} \cdot {\rm d}s_{\text{d}}^{{s_{\text{d}}^{*} }} } \right)} \right)_{{5 \times H_{\text{P}} }} .$$

(16)

Assuming that the function $f_{\text{cd}}^{{s_{\text{d}} }}$ of the spatial vehicle dynamics model is continuous at every point in its domain, the optimal vehicle motion states can be approximately acquired through linearly interpolating ${\boldsymbol{\xi}}_{\text{c}}^{{s_{\text{d}} }}$ to any intermediate point:

$$\begin{aligned} {\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{t} } \right) \triangleq {\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } + n \cdot {\rm d}s_{\text{d}}^{{s_{\text{d}}^{*} }} } \right) + \left( {s_{\text{d}}^{t} - {\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } + n \cdot {\rm d}s_{\text{d}}^{{s_{\text{d}}^{*} }} } \right)} \right) \\ \quad \times {{\left( {{\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } + \left( {n + 1} \right) \cdot {\rm d}s_{\text{d}}^{{s_{\text{d}}^{*} }} } \right) - {\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } + n \cdot {\rm d}s_{\text{d}}^{{s_{\text{d}}^{*} }} } \right)} \right)} \mathord{\left/ {\vphantom {{\left( {{\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } + \left( {n + 1} \right) \cdot {\rm d}s_{\text{d}}^{{s_{\text{d}}^{*} }} } \right) - {\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{ * } + n \cdot {\rm d}s_{\text{d}}^{{s_{\text{d}}^{*} }} } \right)} \right)} {ds_{\text{d}}^{{s_{\text{d}}^{*} }} }}} \right. \kern-0pt} {{\rm d}s_{\text{d}}^{{s_{\text{d}}^{*} }} }}, \\ \quad \quad \forall s_{\text{d}}^{ * } + n \cdot {\rm d}s_{\text{d}}^{{s_{\text{d}}^{*} }} \le s_{\text{d}}^{t} s_{\text{d}}^{ * } + \left( {n + 1} \right) \cdot {\rm d}s_{\text{d}}^{{s_{\text{d}}^{*} }} , \\ \end{aligned}$$

(17)

where $s_{\text{d}}^{t}$ is the projected vehicle position along the reference trajectory at time t.

Due to the vehicle dynamic response, the motion states will not follow exactly the desired trajectory. It still takes some time for the states to converge to the reference values, even if they remain constant in the process. Therefore, the control objective of the motion follow layer is to track the optimal motion states ${\varvec{x}}_{\text{c}}^{\text{ref}}$ at a specific position, which is chosen as $s_{\text{d}}^{t}$ after two time steps (i.e. $s_{\text{d}}^{{t + 2T_{\text{s}} }}$) in this paper, given by

$${\varvec{x}}_{\text{c}}^{\text{ref}} = {\varvec{x}}_{\text{c}} \left( {s_{\text{d}}^{{t + 2T_{\text{s}} }} } \right) = \left( {\begin{array}{*{20}c} {V_{{x{\text{r}}}} } & {V_{{y{\text{r}}}} } & {\dot{\psi }_{\text{r}} } & {e_{{\psi {\text{r}}}} } & {e_{{y{\text{r}}}} } \\ \end{array} } \right)^{\text{T}} ,$$

(18)

$$s_{\text{d}}^{{t + 2T_{\text{s}} }} \triangleq\, s_{\text{d}}^{t} + 2T_{\text{s}} {{v_{\text{s}} \left( t \right)} \mathord{\left/ {\vphantom {{v_{\text{s}} \left( t \right)} {\left( {1 - {{e_{y} \left( t \right)} \mathord{\left/ {\vphantom {{e_{y} \left( t \right)} {\rho \left( t \right)}}} \right. \kern-0pt} {\rho \left( t \right)}}} \right)}}} \right. \kern-0pt} {\left( {1 - {{e_{y} \left( t \right)} \mathord{\left/ {\vphantom {{e_{y} \left( t \right)} {\rho \left( t \right)}}} \right. \kern-0pt} {\rho \left( t \right)}}} \right)}},$$

(19)

where the subscript of (·)_r denotes the reference vehicle states here and hereinafter. Note that the vehicle motion in the longitudinal direction varies more slowly than that in the lateral or yaw direction. Therefore, the reference sampling point $s_{\text{d}}^{{t + 2T_{\text{s}} }}$ is not included in the optimal motion states as the destination position in order to avoid “aggressive” longitudinal control effort.

Note that the MPC with a large prediction horizon tends to exert smooth control actions, so it is reasonable to make the assumption that the time derivatives of the linear and angular velocities are close to zero when the time step T _s is small enough. Then the time derivative of ${\varvec{x}}_{\text{c}}^{\text{ref}}$ is expressed as

$${\dot{\varvec{x}}}_{\text{c}}^{\text{ref}} = \left( {\begin{array}{*{20}c} 0 & 0 & 0 & {\ddot{\psi }_{\text{r}} } & {V_{{x{\text{r}}}} \sin } \\ \end{array} e_{{\psi {\text{r}}}} + V_{{y{\text{r}}}} \cos e_{{\psi {\text{r}}}} } \right)^{\text{T}} .$$

(20)

3.3 Terminal Sliding Mode Based Motion Follower

In the design of the motion follower, the nonlinearity of the vehicle dynamics is usually handled by employing the sliding mode control (SMC) method. Thus, the longitudinal, lateral, and yaw errors follow the dynamics embedded in three sliding surfaces leading to convergence of these variables. A terminal SMC (TSMC) is a type of sliding control, in which the finite-time convergence and quick responsiveness can be achieved on the terminal sliding manifold [28]. Here, the global fast TSMC proposed by PARK et al. [29] is employed to design the sliding surface s ₁ while the nonsingular TSMC technique proposed by FENG et al. [30] is applied to design the sliding surfaces s ₂ and s ₃. The sliding surfaces are defined by

$$\begin{aligned} s_{1} & \equiv \left( {\dot{V}_{x} - \dot{V}_{{x{\text{r}}}} } \right) + \alpha_{1} \left( {V_{x} - V_{{x{\text{r}}}} } \right) + \beta_{1} \left( {V_{x} - V_{{x{\text{r}}}} } \right)^{{{{p_{1} } \mathord{\left/ {\vphantom {{p_{1} } {q_{1} }}} \right. \kern-0pt} {q_{1} }}}} , \\ s_{2} & \equiv \alpha_{2} \left( {e_{y} - e_{{y{\text{r}}}} } \right) + \beta_{2} \left( {V_{y} - V_{{y{\text{r}}}} } \right)^{{{{p_{2} } \mathord{\left/ {\vphantom {{p_{2} } {q_{2} }}} \right. \kern-0pt} {q_{2} }}}} , \\ s_{3} & \equiv \alpha_{3} \left( {e_{\psi } - e_{{\psi {\text{r}}}} } \right) + \beta_{3} \left( {\dot{\psi } - \dot{\psi }_{\text{r}} } \right)^{{{{p_{3} } \mathord{\left/ {\vphantom {{p_{3} } {q_{3} }}} \right. \kern-0pt} {q_{3} }}}} , \\ \end{aligned}$$

(21)

where α _k and β _k are the strictly positive constants of TSMC (k = 1, 2, 3); p _k and q _k are the positive odd numbers and p _k  > q _k . In particular, for k = 2, 3, we also have: 1 < p _k /q _k  < 2.

Because the longitudinal velocity follows the dynamics of a first-order system, the control law u ₁ is directly derived from the sliding surface by letting s ₁ be equal to “0”. Meanwhile, the lateral motion and the yaw motion follow the dynamics of the second-order systems, thus the control laws are obtained from the differential equations, expressed as

$$\dot{s}_{k} = u_{{k{\text{n}}}} ,\forall k \in \left\{ {2,3} \right\},$$

(22)

where u _kn is the approaching law for the sliding surfaces s ₂ and s ₃. Here, the nonlinear reaching law proposed by REN et al. [31] rather than a linear one is adopted to achieve the finite-time convergence of the states when $\dot{s}_{k} = 0$, and thus allows us to have a better control performance:

$$u_{{k{\text{n}}}} = - \alpha_{{k{\text{n}}}} s_{k} - \beta_{{k{\text{n}}}} s_{k}^{{{{q_{{k{\text{n}}}} } \mathord{\left/ {\vphantom {{q_{{k{\text{n}}}} } {p_{{k{\text{n}}}} }}} \right. \kern-0pt} {p_{{k{\text{n}}}} }}}} ,\forall k \in \left\{ {2,3} \right\},$$

(23)

where α _kn and β _kn are the strictly positive constants; p _kn and q _kn are the positive odd numbers and p _kn > q _kn.

To sum up, the control laws are given by

$$\begin{aligned} F_{{x{\text{d}}}} = u_{1} & = m\left( {\frac{{ \rho_{0} C_{\text{D}} A_{\text{f}} V_{x}^{2} }}{2m} - V_{y} \dot{\psi } - \alpha_{1} \left( {V_{x} - V_{{x{\text{r}}}} } \right) - \beta_{1} \left( {V_{x} - V_{{x{\text{r}}}} } \right)^{{{{p_{1} } \mathord{\left/ {\vphantom {{p_{1} } {q_{1} }}} \right. \kern-0pt} {q_{1} }}}} } \right), \\ F_{{y{\text{d}}}} = u_{2} & = m\left( {\left( {V_{x} \dot{\psi }} \right) - } \right.\frac{{\alpha_{2} }}{{\beta_{2} p_{2} }}\left( {V_{y} - V_{{y{\text{r}}}} } \right)^{{1 - {{p_{2} } \mathord{\left/ {\vphantom {{p_{2} } {q_{2} }}} \right. \kern-0pt} {q_{2} }}}} \\ & \quad \left. { \times \left( {V_{x} \sin \left( {e_{\psi } } \right) + V_{y} \cos \left( {e_{\psi } } \right) - V_{{x{\text{r}}}} \sin \left( {e_{{\psi {\text{r}}}} } \right) - V_{{y{\text{r}}}} \cos \left( {e_{{\psi {\text{r}}}} } \right)} \right)} \right), \\ M_{{z{\text{d}}}} = u_{3} & = I_{z} \cdot \frac{{\alpha_{3} q_{3} }}{{\beta_{3} p_{3} }} \cdot \left( {\dot{\psi } - \dot{\psi }_{\text{d}} } \right)^{{2 - {{p_{3} } \mathord{\left/ {\vphantom {{p_{3} } {q_{3} }}} \right. \kern-0pt} {q_{3} }}}} \cdot \left( {\frac{{ - \alpha_{{3{\text{n}}}} s_{3} - \beta_{{3{\text{n}}}} s_{3}^{{{{q_{{3{\text{n}}}} } \mathord{\left/ {\vphantom {{q_{{3{\text{n}}}} } {p_{{3{\text{n}}}} }}} \right. \kern-0pt} {p_{{3{\text{n}}}} }}}} }}{{\alpha_{3} \left( {\dot{\psi } - \dot{\psi }_{\text{d}} } \right)}} - 1} \right). \\ \end{aligned}$$

(24)

The control frequency of the TSMC based motion follower is set to 100 Hz, which is much higher than that of the SMPC based path planner (2 Hz), such that the system stability and the tracking performance at high speed are well guaranteed. To prove the nonlinear stability and the finite-time convergence of the global fast TSMC as well as the nonsingular TSMC, interested readers please refer to Refs. [6] and [32] for details.

4 Results and Discussion

The reference trajectory presented by FALCONE et al. [19] is employed to perform an emergency double-lane-change (DLC) maneuver. The objective of this DLC test is to evaluate the behavior of the proposed control strategy in critical situations. Some of the vehicle and controller parameters are listed in Table 1.

Table 1 List of vehicle and controller parameters

The responses of the vehicles with different control configurations are compared with one another. Here and hereinafter, the controllers using the SMPC presented in section 3.1 with the nonlinear constrained CA (NCCA) proposed by ONO et al. [2] and the linear constrained CA (CCA) in Ref. [6] are referred to as “SMPC + NCCA” and “SMPC + CCA”, respectively. The controller using the hierarchical structure presented in section 3.2 with CCA is referred to as “SMPC + TSMC + CCA”. The integrated chassis controller proposed by SONG et al. [6] with the optimal preview acceleration (OPA) driver model proposed by GUO et al. [33] tracking the desired path and the reference speed is referred to as “OPA + TSMC + CCA”.

4.1 Off-Line Simulation

At this stage, all the controllers as well as the vehicle dynamics model are developed in the MATLAB/Simulink environment and validated through off-line simulations. The results are shown in Figs. 5, 6, 7, 8, 9, and 10 and Tables 2 and 3.

Fig. 5

Reference trajectory tracking

Fig. 6

Reference yaw angle tracking

Fig. 7

Reference longitudinal velocity tracking

Fig. 8

Actuator efforts of four wheels by “SMPC + NCCA”

Fig. 9

Actuator efforts of four wheels by “SMPC + CCA”

Fig. 10

Actuator efforts of four wheels by “SMPC + TSMC + CCA”

Table 2 Performance evaluation of different controllers

Table 3 Elapsed CPU time of MATLAB simulation

As can be seen in Fig. 5, the maximum lateral deviation occurs just before the vehicle with each controller enters into the second straight section of the DLC path. The proposed SMPC controllers with CCA and NCCA yield almost the same control effects and their lateral position errors never exceed 0.65 m. Even though “SMPC + TSMC + CCA” has the smaller maximum lateral offset of 0.6 m, yet its tracking accuracy is inferior to the former ones, which results in a larger root-mean-square (RMS) value of e _y (see Table 2). Nevertheless, the hierarchical controller still significantly improves the vehicle’s tracking performance comparing to “OPA + TSMC + CCA”. It should be pointed out that the reference trajectory is not used directly in the SMPC formulation. The desired lane is assumed to be known only over the preview distance $ds_{\text{d}}^{{s_{\text{d}}^{*} }} \times H_{\text{P}}$ and is obtained by some road information acquisition algorithm.

Fig. 6 shows the yaw angles of these vehicles with respect to the longitudinal position. Because of the quick responsiveness and high steady-state precision of the TSMC based motion follower, the yaw angle by “SMPC + TSMC + CCA” is well matched with the reference road heading angle. The maximum orientation error is less than 2.17°, which occurs in roughly the same location with the maximum lateral offset and is the smallest among all schemes under study. The RMS value of the yaw errors by this hierarchical controller is also the smallest as presented in Table 2.

Fig. 7 shows the evolution of the reference and the actual longitudinal and lateral velocities throughout the simulation. Although the generated speed limit profiles change rapidly with respect to time, yet the vehicle with SMPC control increases or decreases the speed gradually but still follows the reference closely. Among them, “SMPC + TSMC + CCA” provides better speed-tracking performance than the other two controllers while maintaining the vehicle directional stability. The sideslip angle reaches the maximum (absolute) value of 4.08° when the longitudinal velocity drops to 16.87 m/s. In the meanwhile, the vehicle with “OPA + TSMC + CCA” loses its lateral stability and the maximum sideslip angle reaches 15.92° when the longitudinal velocity drops to 14.09 m/s.

Figs. 8, 9, and 10 show the distributed actuator efforts of the four wheels by “SMPC + NCCA”, “SMPC + CCA”, and “SMPC + TSMC + CCA”, respectively. It can be found that the CCA algorithms developed by SONG et al. [6] yields smother control actions than NCCA, where the μ rate of each tire is controlled to the same value [2]. The MC efforts become “aggressive” by introducing the TSMC based motion follower, the parameters of which should be finely tuned in order to mitigate this effect.

Table 3 presents the recorded CPU time consumed by each controller, the algorithm of which is executed on an Intel Core i7-4710MQ CPU with 4 GB RAM that runs at 2.50 GHz under Microsoft Windows 7. It can be seen that the hierarchical control structure brings up to 1700% improvement in computational efficiency. The “SMPC + TSMC + CCA” controller has an approximate elapsed time compared with that of the simple “OPA + TSMC + CCA” controller, and thus satisfies the real-time requirement of on-board applications.

4.2 Real-Time Simulation

When the vehicle operates under less severe handling conditions, the performance of the path-following control cannot be improved further by applying the control of the longitudinal velocity. Hence, the effectiveness of the controller should better be investigated on a test vehicle at high speeds. However, it is of great risk to perform such experiments with a full drive-by-wire prototype vehicle. For safety concerns, the control algorithm is verified through a hardware in-the-loop simulation system.

Because of high computational burden of “SMPC + NCCA” and “SMPC + CCA”, only the algorithms of “SMPC + TSMC + CCA” and “OPA + TSMC + CCA” can possibly be implemented on the real-time controller, i.e. the PXI controller running LabVIEW and its real-time module. CarSim is employed for the full car simulation and controller validation, in which a high-fidelity A-class hatchback model is selected and modified into a full drive-by-wire vehicle. Figs. 11, 12, and 13 show the simulation results.

Fig. 11

Real-time simulation results

Fig. 12

Comparisons of tracking errors

Fig. 13

Comparisons of vehicle motion states

Fig. 11 presents the path-following performance of the controllers in the real-time simulations. The arrows point to the vehicle’s heading direction at each sampling time, while their sizes give a rough indication of the magnitude of the longitudinal velocity. Unlike the off-line simulation results as shown in Table 1, the vehicle with “SMPC + TSMC + CCA” gains a shorter tracking time than that with “OPA + TSMC + CCA”. The average vehicle speed is increased by 0.26 km/h. This is primarily because the cornering strategies employed by the two controllers are quite different from one another: ① The non-preview method is used by the OPA driver model for the speed-tracking control, and brakes the vehicle after passing the first corner resulting in a higher average speed during the first half of the maneuver; ② The “slow-in, fast-out” technique is attained by the proposed controller, and makes the vehicle rotate easier and get higher corner exit speed. As a result, the vehicle with “OPA + TSMC + CCA” tends to be less stable before entering into the second straight section of the path and consequently causes larger tracking errors. By taking account of the lateral deviations in the portion of the path from 55 m to 160 m, the RMS values of e _y can be reduced by 41% through the use of the proposed control method.

Figs. 12 and 13 respectively compare the tracking errors and the motion states of the two controllers. The vehicle driving at a constant speed (72 km/h) is also added for comparison. It can be seen that better tracking performance and handling stability can be achieved with the longitudinal velocity adapted to the road geometry. The vehicle with “SMPC + TSMC + CCA” has the smallest deviation and orientation errors. The RMS value of the orientation errors is reduced by 30% and 68% compared to those attained by the OPA driver model with the non-preview and constant speed control methods, respectively. However, unlike the situation in the off-line simulations, the proposed controller yields slightly larger sideslip angles, the peak (absolute) value of which is suppressed to 1.9°.

5 Conclusions

• (1) A novel approach is devised for the automated lane guidance of a full drive-by-wire vehicle traveling at varying speeds, such that the vehicle can follow a given path as closely and quickly as possible while its stability can be well guaranteed in extreme handling situations.

• (2) For explicit acquisition of the vehicle position as well as the tracking errors over the prediction horizon, the prediction model utilizes the improved variable step-size spatial transformation to derive the dynamics of the vehicle about the reference trajectory.

• (3) In the path planner, the NMPC problem is formulated to compute the sequence of the optimal motion states for path following. With sets of off-line simulations, the SMPC can obtain the best path-following performance of the full drive-by-wire vehicle with the deviation and orientation errors being the smallest among all schemes under study. Moreover, the reference speed is appropriately generated and accurately tracked to maintain the vehicle with good handling stability.

• (4) The TSMC based motion follower is designed to calculate the resultant forces and moment of the vehicle so as to track the generated optimal states. The MC efforts strictly confined within a convex set are readily distributed to the four wheels by using either the NCCA or the CCA tire force distribution method, of which the latter exerts relatively smoother control actions.

• (5) The hierarchical control structure is able to bring up to 1700% improvement in the computational efficiency without sacrificing its control performance. Therefore, the proposed hierarchical “SMPC + TSMC + CCA” algorithm can be implemented on a real-time controller. DLC test results show that the RMS values of the lateral deviations and the orientation errors can be reduced by 41% and 30%, respectively, comparing to those by the simple OPA driver model with the non-preview speed-tracking method. Also the full drive-by-wire vehicle with “SMPC + TSMC + CCA” completes the route in a shorter time with the peak sideslip angle suppressed to 1.9°.