Keywords

These keywords were added by machine and not by the authors. This process is experimental and the keywords may be updated as the learning algorithm improves.

1 Introduction

An enormous amount of work has been devoted during the past three decades to the development of active safety systems for passenger automobiles. This effort has led to the development of a plethora of active safety systems, such as ABS, TCS, ESP, RCS, AFS and others [2, 11, 34, 39], many of which are now standard equipment in production vehicles. The main goal of all these systems is to help the driver avoid, or prevent, the so-called “abnormal” driving scenarios (skidding, sliding, excessive under/oversteer, etc). In these conditions, nonlinear effects dominate the vehicle dynamics, and the tire friction is very close to (or exceeds) the adhesion limit(s). Driving at the boundary of the adhesion limits of the tires leads to a reduced operational stability margin for the driver. The main goal of most current active safety systems is therefore to restrict the operational envelope of the vehicle and the tires inside a linear, well-defined, stable regime. This is, however, an overly conservative approach. Enhanced stability comes at the cost of decreased maneuverability. There are many realistic scenarios where the occurrence (or the post-effects) of a collision can be alleviated by allowing (or even inducing) the vehicle to operate in its nonlinear regime in a controlled manner.

The previous observations naturally lead one to investigate algorithms that exploit the increased vehicle maneuverability brought about by operating the vehicle in nonlinear and/or unstable regimes. By extending the region of validity of the future generation of active safety systems one expects to increase their performance. In our previous work [6, 7, 35–38] we have investigated the mathematical modeling of vehicles operating in nonlinear and/or unstable regimes, and have demonstrated the potential benefits of such an approach to achieve collision avoidance and mitigation beyond what is possible with current active safety systems.

This point of view represents a philosophical departure from current practice, and differs significantly in scope from standard active safety system design for passenger vehicles. As a result—and understandably so—it brings along with it a slew of unanswered questions; among them, the key question is how to generate the necessary control actions (at the short time scales required) that are needed to perform such extreme maneuvers. Indeed, most drivers—except perhaps expert professional, stunt and race drivers—would have great difficulty initiating an aggressive maneuver and controlling the vehicle throughout the whole maneuver duration.

Optimal control is a powerful framework that has been used successfully in many engineering applications to generate feasible trajectories subject to constraints and complicated system dynamics. The field of numerical optimal control has experienced enormous advances during the recent years, to the point that we now have reliable numerical algorithms to generate optimal trajectories for a variety of practical engineering problems [4]. Despite these advances, the current state-of-the-art in numerical optimal control mainly focuses on generating only open-loop optimal controllers. Furthermore, and unless the underline problem (dynamics, cost) obeys certain convexity and/or linearity conditions, current trajectory optimizers do not allow the computation of optimal trajectories in real-time, at least for applications similar to the one we have in mind in this chapter, where the time allotted to solve the problem is in the order of a few milliseconds. One example where fast computation has become possible owing to the current advancement of embedded computing is the area of Model Predictive Control (MPC), where successive linearizations of the plant are used to generate a sequence of linear or convex optimization problems over a finite horizon that can be solved very efficiently on-line [10]. In general, however, optimal solutions for general nonlinear systems and general cost functions are notoriously sensitive to the provided initial guesses and, in the absence of timely re-planning, the robustness of these open-loop optimal control laws is questionable.

Consequently, several researchers have recently turned their attention to the generation of near-optimal trajectories using alternative methods, which bypass the exact on-line computations required for the solution of complicated, nonlinear optimal control problems, opting instead for approximate, near-optimal solutions. One typical approach uses interpolation over pre-computed optimal control actions for a variety of initial conditions. Naïve interpolation, however, does not ensure feasibility—let alone optimality—of the resulting interpolated trajectories. In [1], for instance, the authors used traditional interpolation over pre-computed optimal trajectories. However, this method turns out to be inaccurate and time-consuming. Another, more promising, approach is the one proposed in [14], where the optimal control problem is cast as one of metamodeling, in which the (unknown) map between control inputs/system response pairs is generated implicitly via a series of computer experiments. Specifically, the approach in [14] considers the solution to an optimal control problem obtained by numerical methods as the output of such a metamodel obtained by a series of off-line simulations. A vast number of publications about metamodeling of computer experiments can be found in the literature. Most of them are motivated by the low time-consuming optimization process, derived from having a metamodel of a given simulation.

In contrast to [1], the framework in [14] is based on rigorous interpolation between the off-line solutions (the “metamodel”) using ideas from statistical interpolation theory via Gaussian processes, which in geostatistics it is also known as kriging [8, 16, 22, 33]. Kriging approximates a function observed at a set of discrete points with a convex combination of the observations so as to reduce the least mean-squared error (MSE), and is a special case of prediction using Gaussian processes [16, 22]. Although classical interpolation focuses on low-order polynomial regression, which is suitable for sensitivity analysis, kriging is an interpolation technique that provides better global predictions than classical methods [20, 33]. In this work, we use kriging to construct a (near-)optimal feedback controller from off-line computed extremal trajectories. Prior use of kriging has been focused mainly on simulation and metamodeling [18, 30, 32]. A brief overview of interpolation using Gaussian processes and kriging is given in Sect. 7.3.

We apply a technique similar to the one proposed in [14] to obtain near-optimal “feedback” controllers for the problem of minimum-time aggressive yaw maneuver generation for a high-speed vehicle impeding a collision with another vehicle at an intersection (T-Bone collision). Our results show that kriging interpolation is able to generate very accurate parameterized trajectories in real-time, and hence it may be a potential option for real-time, near-optimal trajectory generation under such extreme driving conditions, where the time constraints do not allow the computation of an exact optimal trajectory in a timely manner using current state of technology.

Prior similar work that uses parameterized trajectory generation includes [9], which developed an algorithm to generate a whole set of trajectories between two pre-computed solutions for two different initial conditions, and [31], where parameterized trajectories were generated using experimental demonstrations of the maneuver. However, the control laws obtained in [9, 31] are open-loop and thus susceptible to uncertainties in the initial conditions and unknown model parameters. The advantage of the method described in this chapter is that the control is obtained as a function of the actual state, hence is a “feedback” control.

The chapter is structured as follows. In the next section the problem to be investigated is introduced, along with the dynamical model of the vehicle and the tire friction dynamics. Next, the optimal control problem is formulated, which is solved over a discrete grid of initial conditions. This series of generated solutions at several discrete points is stored in memory, and is used in Sect. 7.4 to generate a feedback control by interpolating between the stored solutions on-line using kriging. For the benefit of the uninformed reader, a brief summary of kriging theory as used in this chapter is given in Sect. 7.3. In Sect. 7.4.2 we present numerical results from the application of the proposed approach to the problem of T-Bone collision mitigation at an intersection between two speeding vehicles, as a demonstration of the possibilities enabled by the proposed approach for optimal on-line controller generation.

2 Aggressive Yaw Maneuver of a Speeding Vehicle

2.1 Problem Statement

One of the most lethal collisions between two speeding vehicles is the so-called “T-bone” collision (Fig. 7.1), which occurs when one of the vehicles drives into the side of the other vehicle [29]. The vehicle suffering the frontal impact is often referred to as the “bullet” vehicle, while the one suffering the side impact is said to have been “T-boned.” If there is inadequate side impact protection, the occupants of a T-boned vehicle risk serious injury or even death.

Although the bullet vehicle is driving much faster, this collision scenario is specially dangerous for the driver or the side passenger of the target vehicle. This is owing to the fact that the requirements in terms of frontal crashworthiness of cars on the market nowadays is excellent [12]. Frontal-crash tests are carried out at velocities up to 64 km/h, with the result of the passengers cabin being almost intact. The suitable design and choice of materials of the front part of the vehicle allows large structural deformations and thus absorption of the residual energy during impact. Moreover, the installation of frontal airbags, mandatory in the US since September 1998, has resulted in a great decrease of deaths and injuries owing to frontal collisions. On the other hand, the side part of the chassis is structurally weak, and large deformations would result in lethal injuries for the occupants. Unfortunately, side airbags are currently available only in upscale or mid-range cars, although it is envisioned that they will also become standard safety equipment for all passenger vehicles in the future. Reference [13] offers a detailed study on occupant injuries during side impact crashes. As expected, the most frequent source of severe injuries is the contact between the chest and the door panel.

Car manufactures are aware of the high risk involved in side collisions. Volvo, for instance, in 1991 introduced a special protection system against side collision named Side Impact Protection System (SIPS). Other car manufacturers have introduced similar passive safety systems.

In our previous work [6, 7] we investigated the possibility of mitigating the results from an unavoidable T-Bone collision by using an aggressive yaw maneuver for the incoming bullet vehicle. The proposed collision mitigation maneuver involves a rapid yaw rotation of the bullet vehicle at an approximately 90 deg angle that brings the longitudinal axes of the two vehicles into a nearly parallel alignment, in order to distribute the residual kinetic energy of the collision over a larger surface area, thus mitigating its effects. Although this represents a worst-case scenario, where the target vehicle does not respond (a more optimal strategy would involve a rapid yaw maneuver of the target vehicle as well), our initial study focuses only on the case when the bullet vehicle is actively maneuvered during the pre-collision phase. The generalization to the case when both vehicles collaboratively try to avoid the collision will probably involve some vehicle-to-vehicle (V2V) communication and it is left for future investigation. Henceforth, we thus only consider the problem when only one (the bullet) vehicle is actively controlled. This problem was posed in [6, 7] as a time-optimal control problem, and it was solved using pseudospectral methods [25]. In the next two sections we briefly summarize the problem definition and its numerical solution.

Fig. 7.1
figure 1

T-Bone collision

Full size image
Fig. 7.2
figure 2

Schematic of bicycle model

Full size image

2.2 Vehicle and Tire Model

The model used in this chapter is the so-called “bicycle model” [26], augmented with wheel dynamics. The nomenclature and conventions regarding this model are shown in Fig. 7.2. The state is given by \(\mathbf {x}= [u, \textit{v}, r, \psi , \omega _f, \omega _r]^{\text{ T }}\), where \(u\) and \(\textit{v}\) are, respectively, the body-fixed longitudinal and lateral velocities, \(r\) is the vehicle yaw rate, \(\psi \) is the vehicle heading, and \(\omega _f \ge 0\) and \(\omega _r \ge 0\) are the angular speeds of the front and rear wheels, respectively. The system is controlled by \(\mathbf {u}= [\delta , T_b, T_{hb}]^{\text{ T }}\), where \(\delta \) is the steering angle and \(T_b\), \(T_{hb}\) denote the torques generated by the footbrake and handbrake, respectively.

The equations of motion of the vehicle can be written as shown in (7.1)–(7.4)

$$\begin{aligned} \dot{u}&= \frac{1}{m}(F_{xf}\cos \delta - F_{yf}\sin \delta + F_{xr}) + \textit{v}r, \end{aligned}$$
(7.1)
$$\begin{aligned} \dot{\textit{v}}&= \frac{1}{m}(F_{xf}\sin \delta + F_{yf}\cos \delta + F_{yr}) - ur, \end{aligned}$$
(7.2)
$$\begin{aligned} \dot{r}&= \frac{1}{I_z}\big (\ell _f (F_{xf}\sin \delta + F_{yf}\cos \delta ) - \ell _r F_{yr} \big ),\end{aligned}$$
(7.3)
$$\begin{aligned} \dot{\psi }&= r, \end{aligned}$$
(7.4)

along with the wheel dynamics

$$\begin{aligned} \dot{\omega }_f&= \frac{1}{I_w}(T_{bf} - F_{xf} R),\end{aligned}$$
(7.5)
$$\begin{aligned} \dot{\omega }_r&= \frac{1}{I_w}(T_{br} - F_{xr} R), \end{aligned}$$
(7.6)

where \(m, I_z\) are, respectively, the mass and yaw moment of inertia of the vehicle, \(I_w\) is the rotational inertia of each wheel about its axis, \(R\) is the effective tire radius, and \(\ell _f, \ell _r\) are, respectively, the distances of the front and rear axles from the vehicle center of mass. In (7.1)–(7.6) \(F_{ij}\) (\(i=x,\,y\); \(j=f,\,r\)) denote the longitudinal and lateral force components developed by the tires, defined in a tire-fixed reference frame.

These forces depend on the normal loads on the front and rear axles, \(F_{zf}\) and \(F_{zr}\), given by

$$\begin{aligned} F_{zf}&= \frac{m g \ell _r - h m g \mu _{xr}}{\ell _f + \ell _r + h (\mu _{xf}\cos \delta - \mu _{yf}\sin \delta - \mu _{xr})}, \end{aligned}$$
(7.7)
$$\begin{aligned} F_{zr}&= \frac{m g \ell _f + h m g (\mu _{xf}\cos \delta - \mu _{yf}\sin \delta )}{\ell _f + \ell _r + h (\mu _{xf}\cos \delta - \mu _{yf}\sin \delta - \mu _{xr})} \end{aligned}$$
(7.8)

where \(h\) is the distance of the vehicle center of mass from the ground (see Fig. 7.3), and where

$$\begin{aligned} \mu _j = D \sin (C \arctan (B s_j)), \quad \mu _{ij} = -({s_{ij}}/{s_j})\mu _j, \quad i = x,y; j = f,r, \end{aligned}$$
(7.9)

for some constants \(C, B\) and \(D\). Expression (7.9) is a simplified version of the well-known Pacejka “Magic Formula” (MF) [24] for the tire friction modeling, and combines the longitudinal and lateral motion, thus intrinsically incorporating the non-linear effect of the lateral/longitudinal coupling also known as the “friction circle” (see Fig. 7.4), according to which, the constraint \(F_{x,j}^2 + F_{y,j}^2 \le F_{\max ,j}^2 = (\mu _j F_{z,j})^2 ~(j=f,r)\) couples the allowable values of longitudinal and lateral tire friction forces.

Incorporating the friction circle constraint is necessary for the correct modeling of the dynamics occurring during the aggressive maneuvers we consider in this work.

Fig. 7.3
figure 3

Longitudinal load transfer force distribution

Full size image

In Eq. (7.9) \(s_{ij}\) denote the tire longitudinal and lateral slip ratios, given by

$$\begin{aligned} s_{xj} = \frac{V_{xj} - \omega _j R}{\omega _j R} = \frac{V_{xj}}{\omega _j R}-1, \quad s_{yj} = (1 + s_{xj}) \frac{V_{yj}}{V_{xj}}, \qquad j = f,r, \end{aligned}$$
(7.10)

where the longitudinal and lateral velocity components, defined in the tire-fixed reference frame, are given by

$$\begin{aligned}&V_{xf} = u\cos \delta + v\sin \delta + r \ell _f\sin \delta ,\quad V_{yf} = -u\sin \delta + v\cos \delta + r \ell _f \cos \delta ,&\end{aligned}$$
(7.11)
$$\begin{aligned}&V_{xr} = u,\qquad \qquad V_{yr} = v - r \ell _r,&\end{aligned}$$
(7.12)

and \(s\) denotes the total slip, computed as \(s_j = (s_{xj}^2 + s_{yj}^2)^\frac{1}{2}\), (\(j = f,r\)). Finally, the tire forces in (7.1)–(7.6) are computed by \( F_{ij} = F_{zj} \mu _{ij},~(i = x,y; j = f,r)\).

Following current vehicle technology, it is assumed that handbrake torque is only applied on the rear axle and the footbrake torque is distributed to both axles by a factor \(\gamma _b\), according to \({T_{bf}}/{T_{br}}=({1-\gamma _b})/{\gamma _b}\), so that \(T_{bf} = -(1-\gamma _b) T_b\) and \(T_{br} = -\gamma _b T_b - T_{hb}\). It is further assumed that the controls are bounded in magnitude between upper and lower bounds as follows

$$\begin{aligned} \delta _{\min } \le \delta \le \delta _{\max }, \quad 0 \le T_b \le T_{b,\max }, \quad 0 \le T_{hb} \le T_{hb,\max }, \end{aligned}$$
(7.13)

which define the allowable control constraint set, \(\mathbf {u}\in \fancyscript{U}\subset \mathbb {R}^3\). For more details on the vehicle and tire model used in this work, the reader is referred to [7, 35, 38].

Fig. 7.4
figure 4

The friction circle concept (from [19])

Full size image

2.3 Optimal Control Formulation

Assuming that the vehicle is initially moving on a straight line along the positive \(x\) direction with velocity \(V_0=u(0)\), our main goal is to find the control input history \(\mathbf {u}(t)\) to bring the posture of the vehicle to \(\psi (t_f) =90^{\circ }\) as fast as possible. Without loss of generality, it will be assumed that the angular velocity of the front and rear wheels is such that a no-slip condition is satisfied, i.e., \(\omega _f(0) = \omega _r(0) =V_0/R\).

We therefore wish to solve the following optimal control problem

$$\begin{aligned} \underset{\mathbf {u}\in \fancyscript{U}}{\min } ~ J&=\int _0^{t_f} \mathrm {d}t, \end{aligned}$$
(7.14)
$$\begin{aligned} \text {s.t.} ~~~~\dot{\mathbf {x}}&=f(\mathbf {x},\mathbf {u}), \end{aligned}$$
(7.15)
$$\begin{aligned} \mathbf {x}(0)&= [V_0, \, 0, \, 0, \, 0, \, V_0/R, \, V_0/R ]^{\text{ T }}, \end{aligned}$$
(7.16)
$$\begin{aligned} \psi (t_f)&= \pi /2, \end{aligned}$$
(7.17)

where \(f(\mathbf {x},\mathbf {u})\) is given by the right-hand side of (7.1)–(7.6) and, besides \(\psi \), the rest of final states are free.

This problem can be solved using a variety of numerical methods [3–5, 17, 23, 27, 28]. In this work, we have used the package GPOPS based on pseudospectral methods to solve the previous optimal control problem [25]. The problem was solved for a variety of initial conditions. A typical maneuver obtained by the solution of the optimal control problem is shown in Fig. 7.5. For more details, the interested reader is referred to [7]. Table 7.1 summarizes the vehicle model data and tire parameters used in the numerical examples of Sect. 7.4.2.

Fig. 7.5
figure 5

Pre-computed solutions for different initial vehicle velocities (dry asphalt case, corresponding to \(\mu =0.8)\)

Full size image

In the sequel, we focus on generating optimal solutions for different values of initial conditions by interpolating between the pre-computed optimal trajectories. The interpolation method we use is based on representing the input (initial conditions) and output [control commands obtained from the numerical solution of the optimal control problem (7.14)–(7.17)] as a realization of a (hidden) Gaussian process. The goal is then to find the unknown parameters of this Gaussian process in order to predict the optimal control inputs for different problem parameters. Although we only present the results for different initial conditions, the approach can be easily generalized to the case of different vehicle or road parameters, as long as we have enough data points within the range of the parameters of interest.

Table 7.1 Vehicle and tire data used in the numerical simulations
Full size table

3 Statistical Interpolation Using Gaussian Processes

3.1 Basic Theory

The basic idea behind statistical interpolation is that the actual values for all possible observations are a realization from an underlying stochastic process [16]. It is essentially an interpolation technique over random data fields and it provides accurate interpolation even if there is no a priori trend. Kriging is a common term referred to the case when the underlying statistical process is Gaussian. The basic idea that differentiates kriging from the traditional Generalized Least Squares (GLS) approach is the assumption that, given a point where a prediction is to be made, points closer to this new point should have a larger weight, i.e., they should have more influence on the prediction than points that are further away. This implies that the interpolation weights are not constant, but rather they must be specifically computed at each new location.

A kriging interpolation model has the following features:

  1. (a)

    It is unbiased, i.e., the expected value of the error is zero.

  2. (b)

    It is optimal, in the sense that minimizes the variance of the error.

  3. (c)

    It provides exact interpolation, i.e., the predicted output values at the already observed points are equal to the observations.

  4. (d)

    It is computationally very efficient, hence on-line implementation is feasible.

Below we briefly summarize the basic ingredients of the approach. The discussion in this section is taken mainly from [15]. In order to understand how statistical prediction works, let us consider a set of given locations \(\mathbf {X}=[\mathbf {x}_1 \, \ldots \, \mathbf {x}_N]\in \mathbb {R}^{n\times N}\) with \(\mathbf {x}_i \in \mathbb {R}^n\), where an unknown function \(y: \mathbb {R}^n \rightarrow \mathbb {R}\) is observed. A simple regression model is to assume that

$$\begin{aligned} y(\mathbf {x}) = \sum _{k=1}^{r} \beta _k f_k(\mathbf {x}) + z = f(\mathbf {x})^{\text{ T }}{\beta } + z, \end{aligned}$$
(7.18)

for some basis functions (regressors) \(f(\mathbf {x}) = [f_1(\mathbf {x}) \, \ldots \, f_r(\mathbf {x})]^{\text{ T }}\), where \(\beta = [\beta _1\, \ldots \, \beta _r]^{\text{ T }}\in \mathbb {R}^r\) is the vector of regression coefficients, and \(z\in \mathbb {R}\) is the observation error. Let now \(\mathbf {y}=[y(\mathbf {x}_1) \ldots y(\mathbf {x}_N)]^{\text{ T }}=[y_1 \ldots y_N]^{\text{ T }}\in \mathbb {R}^N\) be the vector of observations. The generalized regression model given the data \((\mathbf {y},\mathbf {x}_1,\ldots ,\mathbf {x}_N)\) follows easily from (7.18)

$$\begin{aligned} \mathbf {y}= F(\mathbf {X}) \beta + \mathbf {z}, \end{aligned}$$
(7.19)

where \(\mathbf {z}= [z_1 \, \ldots \, z_N]^{\text{ T }}\in \mathbb {R}^N\) is the vector of observation errors, and \(F(\mathbf {X}) \in \mathbb {R}^{N\times r}\) is the matrix of regressors, given by

$$\begin{aligned} F(\mathbf {X}) = [f(\mathbf {x}_1)^{\text{ T }}\, \ldots \,f(\mathbf {x}_N)^{\text{ T }}]^{\text{ T }}= \left[ \begin{array}{llll} f_1(\mathbf {x}_1) &{} f_2(\mathbf {x}_1) &{} \ldots &{} f_r(\mathbf {x}_1) \\ f_1(\mathbf {x}_2) &{} f_2(\mathbf {x}_2) &{} \ldots &{} f_r(\mathbf {x}_2) \\ \vdots &{} \vdots &{} \ddots &{} \vdots \\ f_1(\mathbf {x}_N) &{} f_2(\mathbf {x}_N) &{} \ldots &{} f_r(\mathbf {x}_N) \end{array}\right] . \end{aligned}$$
(7.20)

In statistical prediction the errors \(\mathbf {z}\) in (7.19) are modeled as a stationary covariance stochastic processFootnote 1 having the properties

$$\begin{aligned} {\mathbb {E}}[\mathbf {z}]&= 0, \end{aligned}$$
(7.21)
$$\begin{aligned} \mathrm{cov }[\mathbf {z}]&={\mathbb {E}}[\mathbf {z}\mathbf {z}^{\text{ T }}]={\mathbf {C}} = \sigma ^2 \mathbf {R}, \end{aligned}$$
(7.22)

where \({\mathbf {C}},\mathbf {R}\in \mathbb {R}^{N\times N}\) are the covariance and correlation matrices, respectively, defined by

$$\begin{aligned} {\mathbb {E}}[z_i z_j] = {\mathbf {C}}_{ij} = \sigma ^2 \mathbf {R}_{ij}(\mathbf {x}_i,\mathbf {x}_j), \quad i,j=1,...,N. \end{aligned}$$
(7.23)

where \(\mathbf {R}_{ij}(\mathbf {x}_i,\mathbf {x}_j)\) are stationary correlation functions to be defined later.

Suppose now that we want to predict the value \(y(\mathbf {x}_0)\) at the new location \(\mathbf {x}_0 \in \mathrm{co }({\mathbf {x}_1,\mathbf {x}_2,\ldots ,\mathbf {x}_N})\), where \(\mathrm{co }(\cdot )\) denotes convex hull. From (7.18), the predicted value of \(y(\mathbf {x}_0)\) is then given by

$$\begin{aligned} y(\mathbf {x}_0) = f(\mathbf {x}_0)^{\text{ T }}\beta + z_0, \end{aligned}$$
(7.24)

where the scalar \(z_0\) represents the prediction error. Here is where kriging and GLS differ. The later assumes that both the sample disturbances in (7.18) and the predictor disturbance in (7.24) are independent, that is, \(\mathrm{cov }[\mathbf {z},z_0]=0\). However, in view of the interdependence of disturbances in the samples (\({\mathbf {C}}\) has non-zero off-diagonal elements), it seems more reasonable to assume that [15]

$$\begin{aligned} {\mathbb {E}}[z_0]=0, \end{aligned}$$
(7.25)
$$\begin{aligned} \mathrm{cov }[z_0]={\mathbb {E}}[z_0^2]&=\sigma ^2, \end{aligned}$$
(7.26)
$$\begin{aligned} \mathrm{cov }[\mathbf {z},z_0]&=\sigma ^2 r(\mathbf {x}_0), \end{aligned}$$
(7.27)

where \(r(\mathbf {x}_0) \in \mathbb {R}^{N}\) is the vector of correlations between \(\mathbf {z}\) and \(z_0\).

Assuming now that the optimal linear predictor of (7.24) can be written in terms of the observed values, one obtains

$$\begin{aligned} \hat{y}(\mathbf {x}_0) = \sum _{i=1}^{N} \textit{w}_i y_i = \mathbf {w}^{\text{ T }}\mathbf {y}, \end{aligned}$$
(7.28)

where \(\mathbf {w}=[\textit{w}_1 \, \ldots \, \textit{w}_N]^{\text{ T }}\in \mathbb {R}^N\) is the column vector of weights. The residual error of the approximation is given by

$$\begin{aligned} \varepsilon (\mathbf {x}_0) = \hat{y}(\mathbf {x}_0) - y(\mathbf {x}_0) = \sum _{i=1}^{N} w_i y_i - {y}(\mathbf {x}_0). \end{aligned}$$
(7.29)

In order to determine the optimal weights \(\mathbf {w}\) kriging imposes the conditions [15, 20]

$$\begin{aligned} \underset{\mathbf {w}}{\min }~ \mathrm{var }[\varepsilon (\mathbf {x}_0)] \qquad \text {s.t.} \quad {\mathbb {E}}[\varepsilon (\mathbf {x}_0)]=0, \end{aligned}$$
(7.30)

to obtain the Best Linear Unbiased Predictor (BLUP). In some texts [20, 21] the criterion involves the minimization of the mean square error instead. It turns out that both criteria are equivalent if the estimator is unbiased.

The minimization problem in (7.30) can be re-written as a quadratic programming (QP) problem in the form

$$\begin{aligned}&\underset{\mathbf {w}}{\min }\,\mathrm{var }[\varepsilon (\mathbf {x}_0)]=\underset{\mathbf {w}}{\min }\, \sigma ^2(1+\mathbf {w}^{\text{ T }}\mathbf {R}\mathbf {w}-2\mathbf {w}^{\text{ T }}r(\mathbf {x}_0)), \nonumber \\&\text {subject to} ~~~F(\mathbf {X})^{\text{ T }}\mathbf {w}- f(\mathbf {x}_0) =0, \end{aligned}$$
(7.31)

whose solution is readily obtained as follows

$$\begin{aligned} \mathbf {w}^*&= \mathbf {R}^{-1}(r(\mathbf {x}_0)-F(\mathbf {X}) \lambda ^*), \end{aligned}$$
(7.32)
$$\begin{aligned} \lambda ^*&= \big ( F(\mathbf {X})^{\text{ T }}\mathbf {R}^{-1} F(\mathbf {X}) \big )^{-1} \big ( F(\mathbf {X})^{\text{ T }}\mathbf {R}^{-1} r(\mathbf {x}_0)-f(\mathbf {x}_0) \big ). \end{aligned}$$
(7.33)

Using the previous expressions, one may finally express the best linear unbiased predictor of (7.28) as

$$\begin{aligned} \hat{y}(\mathbf {x}_0)=\mathbf {R}^{-1}\left[ r(\mathbf {x}_0)-F(\mathbf {X}) \big (F(\mathbf {X})^{\text{ T }}\mathbf {R}^{-1}F(\mathbf {X}) \big )^{-1} \big ( F(\mathbf {X})^{\text{ T }}\mathbf {R}^{-1}r(\mathbf {x}_0)-f(\mathbf {x}_0)) \right] \mathbf {y}. \end{aligned}$$
(7.34)

A deeper insight in the predictor can be obtained by expressing (7.34) as

$$\begin{aligned} \hat{y}(\mathbf {x}_0) = f(\mathbf {x}_0)^{\text{ T }}\beta ^* + r(\mathbf {x}_0)\gamma ^* , \end{aligned}$$
(7.35)

where

$$\begin{aligned} \beta ^* =(F(\mathbf {X})^{\text{ T }}\mathbf {R}^{-1}F(\mathbf {X}))^{-1}F(\mathbf {X})^{\text{ T }}\mathbf {R}^{-1} \mathbf {y},\qquad \gamma ^* = \mathbf {R}^{-1}(\mathbf {y}-F(\mathbf {X})\beta ^*). \end{aligned}$$
(7.36)

The term \(\beta ^*\) is the GLS solution to the regression problem \(\mathbf {y}\approx F(\mathbf {X}) \beta \), also known as Aitken’s GLS estimator [15]. From (7.35) it can be seen that, if independence of the disturbances is considered,that is, \(r(\mathbf {x}_0)=0\), then the solution becomes equivalent to GLS. Another important point is that \(\beta ^*\) and \(\gamma ^*\) are fixed for a given set of design data \(\mathbf {x}_1,\mathbf {x}_2,\ldots ,\mathbf {x}_N\) and \(\mathbf {y}\). Thus the computational effort required to calculate the value of the interpolated function at one point involves only the computation of two vectors (by evaluating the regression basis functions and the correlation function) and two simple products.

As mentioned previously, (7.35) is an exact interpolator, in the sense that it returns the observed value at the design points. This can be easily shown from (7.35) by choosing \(\mathbf {x}_0=\mathbf {x}_i\). Then \(r(\mathbf {x}_i)\) is just the \(i\)th column of the correlation matrix \(\mathbf {R}\). Hence \(\mathbf {R}^{-1}r(\mathbf {x}_i)=e_i\) where \(e_i\) is the \(i\)th column of the identity matrix. It follows that

$$\begin{aligned} \hat{y}(\mathbf {x}_i)&= f(\mathbf {x}_i)^{\text{ T }}\beta ^* + r(\mathbf {x}_i)\mathbf {R}^{-1}\big ( \mathbf {y}-F(\mathbf {X}) \beta ^* \big ) \nonumber \\&= f(\mathbf {x}_i)^{\text{ T }}\beta ^* + e_i(\mathbf {y}-F(\mathbf {X})\beta ^*) \\&= f(\mathbf {x}_i)^{\text{ T }}\beta ^* + y_i-f(\mathbf {x}_i)^{\text{ T }}\beta ^* = y_i. \nonumber \end{aligned}$$
(7.37)

3.2 Choice of Correlation Functions

It is important to emphasize that the accuracy of the method is highly dependent on the choice of correlation functions in (7.23) and (7.27), since they determine the influence of the observed values in the surrounding locations. These are not known a priori, however, and they have to be estimated from the data. In order to find a way to approximate the correlation functions, it is customary to assume that they can be expressed as

$$\begin{aligned} \mathbf {R}_{ij}(\theta ;\mathbf {x}_i,\mathbf {x}_j) = \prod _{k=1}^{n} \rho (\theta ;\mathbf {x}_i^{(k)},\mathbf {x}_j^{(k)}) = \prod _{k=1}^{n} \rho (\theta ;|\mathbf {x}_i^{(k)}-\mathbf {x}_j^{(k)}|), \end{aligned}$$
(7.38)

for some parameter \(\theta \) and \(\mathbf {x}_i,\mathbf {x}_j \in \mathbb {R}^n\) with \(\mathbf {x}^{(k)}\) denoting the \(k\)th component of the vector \(\mathbf {x}\). The expression (7.38) implies that multi-dimensional correlations are expressed as a product of \(n\) one-dimensional correlation functions. Spatial correlation functions depend on both the parameter \(\theta \) and the distance between the considered points \(\ell =|\mathbf {x}_i^{(k)}-\mathbf {x}_j^{(k)}|\). In order to result in proper correlation functions \(\mathbf {R}_{ij}\), the coordinate correlation function \(\rho \) must satisfy \(0 \le \rho (\theta ;\ell ) \le 1\) for all \(\ell \ge 0\). Furthermore, it must satisfy \(\rho (\theta ;0)=1\) and \(\lim _{\ell \rightarrow \infty } \rho (\theta ; \ell )=0\), encoding the fact that far-away points have weaker or no correlation, whereas coincident points yield maximum correlation.

The parameter \(\theta \) determines how fast the correlation function goes to zero. This parameter can be obtained using Maximum Likelihood Estimation (MLE). Figure 7.6 shows the effect of \(\theta \) is the response surface for the function \(f(x,y) = x^2 + y^3\). For simplicity, a constant polynomial is selected as the general trend. Different kriging models are built for different values of \(\theta \). The resulting metamodel representation and the observation points for the decreasing values of \(\theta \) are also shown in Fig. 7.6.

Fig. 7.6
figure 6

Influence of the parameter \(\theta \) in the response surface. a \(\theta =1{,}000\), b \(\theta = 20\), c \(\theta = 5\) d \(\theta \) obtained from MLE

Full size image
Fig. 7.7
figure 7

Different possible choices for correlation functions \(\rho (\theta ;|d|)\)

Full size image

The spatial evolution according to the distance from the origin and the influence of the parameter \(\theta \), for different correlation functions, is shown in Fig. 7.7. As it is customary in practice, the state variables are normalized so that have unit length. Consequently, the normalized support (\(|d| = \ell \)) of \(\rho \) is this figure is \(0 \le |d|\lesssim 2\).

4 Application to On-line Aggressive Vehicle Maneuver Generation

4.1 Feedback Controller Synthesis

Using the method outlined in Sect. 7.2.3, a set of trajectories was computed offline using five equidistant initial conditions corresponding to vehicle initial speeds \(V_0=[40,\, 48,\, 56,\, 64,\, 72]\) km/h. The pre-computed open-loop optimal trajectories for three of the cases considered are shown in Fig. 7.8.

Fig. 7.8
figure 8

Optimal open-loop state trajectories generated with GPOPS

Full size image

We are interested in obtaining a controller able to perform the maneuver described in Sect. 7.2.1 in a (near-)optimal manner for any initial velocity in the interval \(40~\mathrm {km/h} \le V_0 \le 72~\mathrm {km/h}\). To this end, we use the interpolation expressions derived in Sect. 7.3.1, specifically, Eq. (7.35). A separate interpolation model is needed for each variable we want to interpolate. In this case we have a total of four interpolating metamodels: three for the control signals and one more for the optimal final time. A uniform discretization of the optimal trajectories provides the input data \(\mathbf {X}=[\mathbf {x}_1 \, \ldots \, \mathbf {x}_N]\), whereas the control commands \(\delta ,T_b, T_{bh}\), and the final time \(t_f\) comprise the vector of the observed variables \(\mathbf {y}\) at the same time instances. Given now a state \(\mathbf {x}_0 = (u(t_k),\textit{v}(t_k),\psi (t_k),\omega _f(t_k),\omega _f(t_k))\) of the vehicle trajectory at some time \(t_k\), we obtain the required control inputs as a function of the current state as follows

$$\begin{aligned} \delta (t_k)&= \hat{y}_1(\mathbf {x}_0) = \kappa _1(u(t_k),\textit{v}(t_k),\psi (t_k),\omega _f(t_k),\omega _f(t_k)), \end{aligned}$$
(7.39)
$$\begin{aligned} T_b(t_k)&= \hat{y}_2(\mathbf {x}_0) = \kappa _2(u(t_k),\textit{v}(t_k),\psi (t_k),\omega _f(t_k),\omega _f(t_k)), \end{aligned}$$
(7.40)
$$\begin{aligned} T_{bh}(t_k)&= \hat{y}_3(\mathbf {x}_0) = \kappa _3(u(t_k),\textit{v}(t_k),\psi (t_k),\omega _f(t_k),\omega _f(t_k)), \end{aligned}$$
(7.41)

where, for notational convenience, we have introduced, rather informally, the interpolating functions \(\kappa _i\) (\(i=1,2,3,4\)) to denote the right-hand-side of (7.35). Similarly, the optimal time to perform the maneuver from the current state is given by

$$\begin{aligned} t_f(t_k) = \hat{y}_4(\mathbf {x}_0) = \kappa _4(u(t_k),\textit{v}(t)k),\psi (t_k),\omega _f(t_k),\omega _f(t_k)). \end{aligned}$$
(7.42)

Note from (7.39) to (7.41) that the approach yields, at each instant of time, a control action that depends on the current state, that is, the resulting control has a feedback structure. In essence, we have developed a tool for controller synthesis where the open-loop optimal controllers are combined to a single feedback strategy. The difference with standard approaches is that this synthesis is not performed analytically, but rather numerically, via an implicit interpolation of the pre-computed open-loop control laws.

For all computations we have used the DACE toolbox for Matlab [21]. Both the correlation functions and the allowable values for the parameter \(\theta \) were determined by trial and error. Constant and first order polynomials were sufficient for this problem, along with cubic correlation functions (see Fig. 7.7). The optimal value of \(\theta \) was obtained using MSE, as explained in Sect. 7.3.2.

4.2 Numerical Results

The family of near-optimal controls is shown in Fig. 7.9a–c. The red lines highlight the pre-computed solutions used to obtain the interpolating metamodel.

These results show that the controller obtained using the proposed statistical interpolation technique generates near-optimal solutions for the whole range of initial velocities considered. In all simulations the trajectories reach the final constraint, \(\psi =90 ^{\circ }\), as required. Furthermore, notice in Fig. 7.9a–c how the interpolated solutions match the pre-computed ones at the trial sites. This is a consequence of the exact interpolation property of the interpolation scheme, shown in (7.37). Notice also that the solutions vary smoothly along the whole range of initial velocities.

The average to compute a single interpolation of all three controls was \(1.2\) ms (or a rate of \(800\) Hz) on a Intel Pentium Core 2 Duo processor running at 2.4 GHz with 4 GB of RAM. This rate is considered fast enough for real-time controller implementation.

Fig. 7.9
figure 9

Interpolated optimal control histories. a Steering command. b Footbrake command. c Handbrake command

Full size image
Fig. 7.10
figure 10

Effect of correlation parameter \(\theta \) on the optimal footbrake command response surface. a Large value of \(\theta \). b Medium value of \(\theta \). c Low value of \(\theta \)

Full size image

The parameter \(\theta \) that affects the behavior of the correlation functions is shown in Fig. 7.10 for the case of the footbrake command. For very large values of \(\theta \) the solution tends to the GLS solution (\(r(\mathbf {x}_0) = 0\) except at the observation points). The oscillatory behavior for large values of \(\theta \) observed in these figures is owing to the fact that we have chosen a zero order polynomial for the footbrake, in which case the interpolating terms tend to a superposition of impulse functions at the observed points (see Fig. 7.7). As the value of \(\theta \) is reduced, each observation increases its “region of influence” over a larger area of the space, thus “averaging out” the contributions from neighboring observation points.

It is also of interest to explore the positive attributes that arise from having a controller in feedback form [see again (7.39–(7.41)]. Although there is no analytic expression for the feedback controller, the control action is obtained as a function of the current state. Feedback controllers are more desirable than open-loop controllers since they can account for sudden changes in the state, unmodelled uncertainties, etc. In order to evaluate the benefits of having a controller in a feedback form, a simulation with a disturbance representing a 30 % reduction in the yaw rate at \(t=0.6 t_{f}\) was carried out. The comparison was performed at one of the trial locations where the interpolated solution matches the pre-computed one, so the comparison is fair. The initial velocity was chosen as \(V_0 = 56~\mathrm {km/h}\). Figure 7.11 shows how the interpolated control changes when the disturbance is applied and how the system is finally guided to the final constraint despite the abrupt change in the state at \(t=0.6 t_f\).

Fig. 7.11
figure 11

State evolution comparison under disturbance

Full size image

5 Conclusions

The future generation of active safety systems for passenger vehicles will have to take advantage of the nonlinearities of the vehicle and tire friction dynamics in order to safely implement more aggressive obstacle avoidance maneuvers in the case of an impeding accident. Unfortunately, generating optimally such aggressive maneuvers—at the time scales required along with convergence guarantees—is still an elusive goal with current trajectory optimizers. In this chapter we investigate the use of a statistical interpolation technique based on Gaussian processes (e.g., kriging) to generate near-optimal trajectories, along the corresponding control actions, from a set of off-line pre-computed optimal trajectories. The resulting approach essentially generates a metamodel of the action-response map based on the pre-computed optimal control solutions. The resulting interpolation model emulates an optimal feedback controller, as long as the initial conditions are contained in the convex hull of the off-line test locations.

Our numerical results show that the resulting controller has excellent performance, always guiding the system to the exact terminal constraint. Furthermore, the controller is extremely fast to compute, since it is based on simple algebraic manipulations and hence it is beneficial for all similar situations where decisions must be taken within extremely short deadlines.