An Integrated Obstacle Avoidance Controller Based on Scene-Adaptive Safety Envelopes

Abstract

This paper presents an integrated active obstacle avoidance controller in the Model Predictive Control (MPC) framework to ensure adaptive collision-free obstacle avoidance under complex scenarios while maintaining a good level of vehicle stability and steering smoothness. Firstly, with the observed road conditions and obstacle states as inputs, a data-driven Gaussian Process Regression (GPR) model is constructed and trained to generate confidence intervals, as scene-adaptive dynamic safety envelopes represent the safety boundaries of obstacle avoidance. Subsequently, the generated safety envelopes are transformed into soft and hard constraints, incorporated into the MPC controller and rolling updated in the prediction horizon to further cope with uncertain and rapidly evolving driving conditions. Minimizing both the control increments and stability feature parameters are formulated into the objectives of the MPC controller. By solving the multi-objective optimization problem with soft and hard constraints imposed, control commands are obtained to steer the vehicle in order to avoid the obstacles safely and smoothly with guaranteed vehicle stability. The experiments conducted on a motion-base driving simulator show that the proposed controller manages to perform safe and stable obstacle avoidance even under hazardous conditions. It is also verified that the proposed controller can be applied to more complex scenarios with dynamic obstacles presented.

1. Introduction

In recent years, technologies related to Intelligent Connected Vehicles (ICV) have been developed rapidly, showing great potential in addressing traffic safety concerns [1,2,3,4]. Active obstacle avoidance systems can effectively improve traffic safety and have become a research hotspot in the field of ICV [5].

Existing obstacle avoidance controllers are mainly devised in a hierarchical structure, in which the path planner generates a collision-free path, which is then tracked by the lower-level tracking controller [6]. In this hierarchical and sequential framework, paths are usually planned regardless of the complex dynamics of the vehicle, without taking into account the stability feature parameters of the vehicle, such as sideslip angle and yaw rate, as well as the road parameters that affect the stability of the vehicle, such as the road adhesion coefficient [7]. As a result, when the driving conditions are complex, especially when the road conditions change rapidly or the vehicle exhibits prominent nonlinearity, the path planning module may generate infeasible paths due to neglect of road conditions and vehicle state. In this case, the tracking controller is prone to either cause vehicle instability in attempting to accurately track the infeasible path, or lead to collision due to sacrificing tracking accuracy in order to maintain vehicle stability, neither of which are desirable for an obstacle avoidance system.

To address the above issue, some studies have proposed integrating path planning and tracking into a single module and performing path planning and tracking simultaneously by solving a constrained optimization problem with the same vehicle model to ensure that obtained control commands are feasible from the perspective of tracking. Febbo et al. [8] combined path planning and tracking into a single optimization problem by incorporating obstacles in the form of hard constraints into a nonlinear model predictive controller (MPC). Li et al. [9] constructed a motion trend function to predict the position of obstacles in the MPC prediction horizon to optimize the front wheel steering angle for simultaneous path planning and tracking. Guo et al. [10] achieved simultaneous obstacle avoidance path planning and tracking by calculating the ideal obstacle avoidance lateral displacement at a future moment and incorporating this into the optimization objective of the tracking controller. Although the above methods effectively improve the vehicle obstacle avoidance performance, the optimization functions constructed are nonconvex [11] and the constraints are complex, leading to sizeable computational overheads. Meanwhile, these methods tend to fall into local optimality due to the limited horizon. Inspired by the envelope-based control of aircraft, scholars have explored methods using safety envelopes to improve obstacle avoidance performance. Erlien et al. [12,13] constructed a safe driving envelope consisting of both a vehicle stability envelope and the physical boundaries of lanes and obstacles, so that the vehicle is allowed to safely deviate from the path when necessary in order to ensure vehicle stability. However, it is worth noting that the proposed envelope is not continuous and does not match the steering characteristics of the vehicle. More importantly, the kinematics and physical boundary-based envelopes are nonadaptive to arbitrary obstacle avoidance scenarios.

Obviously, effectively constructing safety envelopes is an essential issue in devising scene-adaptive and robust obstacle avoidance controllers. However, it is a rather nontrivial task to obtain the safety envelopes, since the integrated effects of the complex vehicle dynamics and the evolving driving situation, and the probabilistic uncertainty due to modeling errors and inaccuracies in observing the environment, should also be taken into account. The Gaussian process regression (GPR) model has significant advantages in modeling nonlinear dynamical systems with uncertainties, due to its native ability to express probabilistic uncertainty by calculating confidence intervals for the estimates through variance prediction [14]. Therefore, in this paper, a data-driven GPR model is proposed to generate the safety envelope of obstacle avoidance using the distances of the obstacles relative to the ego-vehicle and the road boundaries as inputs.

Furthermore, exploiting the predictive capability of MPC and its unique advantages in rolling optimization when dealing with optimization problems [15,16], this paper proposes to incorporate the constructed safety obstacle avoidance envelope into the framework of MPC and rolling updates the safety envelope in the MPC prediction horizon to further improve the adaptability to uncertain environments. Suh et al. [17] calculate the ideal steering angle to keep the vehicle located within the envelope, which is then tracked by a tracking controller. Brown et al. [13] incorporate the distance between the vehicle and the envelope boundary as a penalty term in the tracking controller objective function. However, the above method still requires calculating the ideal reference trajectory and then balancing the tracking accuracy and stability. Instead of planning a reference trajectory beforehand, we impose the safety envelopes as constraints, so that the controller is allowed to obtain the collision-free control commands within the safety envelopes, subject to optimization objectives of vehicle stability and steering smoothness. Moreover, in order to improve the robustness and safety of the controller, this paper adopts two confidence intervals $(m\pm \sigma )$ and $(m\pm 2\sigma )$ as the soft and hard constraints for obstacle avoidance, respectively. The interval between soft and hard constraints is considered as a buffer area. When the driving condition is challenging, such as on low-adhesion roads, the vehicle can cross the soft constraints to avoid instability by driving in the buffer zone.

The contributions of this paper are summarized as follows:

(1)

A GPR-based vehicle obstacle avoidance safety envelope is proposed, which is more scene-adaptive and consistent with the steering characteristics of the vehicle as compared to envelopes constructed based on explicit metrics and physical boundaries. Moreover, the safety envelope is rolling updated in the MPC prediction horizon. Combined with the advantages of the GPR model in modeling uncertainties, the proposed method can better cope with uncertain and rapidly evolving driving conditions.

(2)

A multi-objective MPC controller incorporating the safety obstacle avoidance envelopes as constraints is proposed. With soft and hard constraints imposed, the MPC controller solves the optimization problem, with vehicle stability and steering smoothness as the objectives, to obtain control commands that guarantee collision-free obstacle avoidance, meanwhile maintaining a good level of vehicle stability and steering smoothness. The experiments prove that, in challenging and dynamic scenes, the stability of the vehicle is significantly improved under the premise of avoiding obstacles safely.

The framework of the proposed obstacle avoidance controller is illustrated in Figure 1. The remainder of the paper is organized as follows. Section 2 describes the methodology for constructing the obstacle avoidance controller. The simulation results and analysis of the proposed obstacle avoidance controller are introduced in Section 3. Finally, conclusions are presented in Section 4.

2. Methodology

2.1. Vehicle Models

This section introduces the vehicle dynamics model of the ego-vehicle and the obstacle vehicle model, both adopted as prediction models for MPC-based obstacle avoidance controllers.

2.1.1. Ego-Vehicle Dynamics Model

Considering the trade-off between computational efficiency and modeling accuracy of the MPC controller, the bicycle model shown in Figure 2 [18] is adopted as the ego-vehicle dynamics model, and the corresponding model parameters are shown in

Table 1. Vehicle Dynamics Parameters.

Description	Symbol
Vehicle Mass	m
Distance from the center of mass to the front/rear axis	$L_f$/$L_r$
Longitudinal force of the front/rear tires	$F_{Xf}$/$F_{Xr}$
Lateral force of front/rear tires	$F_{Yf}{ /F}_{Yr}$
Sideslip angle of front/rear tires	${\alpha }_f{ /\alpha }_r$
Steering angle of the front wheel	${\delta }_f$
Longitudinal/Lateral speed	$v_x/v_y$
Yaw rate	$\dot{\phi }$
Sideslip Angle	$\beta$

.

Analyzing the forces on the model, the expression for the vehicle dynamics force can be obtained as given in Equation (1):

$$\{\begin{array}{c}{\dot{v}}_y=-v_x\dot{\phi }+\frac{2F_{Yr}+2(F_{Yf}\cos ({\delta }_f)+F_{Xf}\sin ({\delta }_f))}{m}\\ {\dot{v}}_x=v_y\dot{\phi }+\frac{2F_{Xr}+2(F_{Xf}\cos ({\delta }_f)-F_{Yf}\sin ({\delta }_f))}{m}\\ \ddot{\phi }=\frac{2L_f(F_{Yf}\cos ({\delta }_f)+F_{Xf}\sin ({\delta }_f))-2L_r{F}_{Yr}}{I_Z}\\ \dot{Y}=v_x\sin \phi +v_y\cos \phi \\ \dot{X}=v_x\cos \phi -v_y\sin \phi \end{array}$$

(1)

where $I_Z$ represents the yaw inertia and $X$ and $Y$ are the vehicle’s longitudinal and lateral positions in the Cartesian coordinate system. When the sideslip angle and the longitudinal slip ratio are small, the tire force is simplified as follows [19]:

$$\{\begin{array}{c}{F}_{Y[f,r]}=C_{c[f,r]}{\alpha }_{[f,r]}\\ F_{X[f,r]}=C_{l[f,r]}{\kappa }_{[f,r]}\end{array}$$

(2)

where $C_{c[f,r]}$ and $C_{l[f,r]}$ represent the cornering and longitudinal stiffness of front (rear) tires, respectively, and ${\kappa }_{[f,r]}$ represents the slip ratio of front (rear) tires, which can be measured when the vehicle is equipped with an Antilock Brake System (ABS).${\alpha }_{[f,r]}$ represents the sideslip angle of front (rear) tires, which can be obtained by analyzing the geometric relationship of the vehicle model shown in Figure 2:

$$\{\begin{array}{c}{\alpha }_f=\frac{v_y+L_f\dot{\phi }}{v_x}-{\delta }_f\\ {\alpha }_r=\frac{v_y-L_r\dot{\phi }}{v_x}\end{array}$$

(3)

The sideslip angle of the vehicle mass can be obtained with the following formula:

$$\beta =\frac{v_y}{v_x}$$

(4)

Combining Equations (1)–(4), the dynamic force equation of the vehicle can be obtained as follows:

$$\{\begin{array}{l}\dot{Y}=v_x\sin \phi +v_{\mathrm{x}}\beta \cos \phi \hfill \\ \dot{\beta }=\frac{-2\left(C_{cr}+C_{cf}\right)}{m{v}_x}\beta +\left(\frac{2C_{cr}{L}_r-2C_{cf}{L}_f}{m{v_x}^2}-1\right)\dot{\phi }+\frac{2C_{cf}}{m{v}_x}{\delta }_f\hfill \\ \dot{\phi }=\dot{\phi }\hfill \\ \ddot{\phi }=\frac{2C_{cr}{L}_r-2C_{cf}{L}_f}{I_z}\beta -\frac{2C_{cr}{L}_r^2+2C_{cf}{L}_f^2}{I_z{v}_x}\dot{\phi }+\frac{2C_{cf}{L}_f}{I_z}{\delta }_f\hfill \end{array}$$

(5)

Setting the state vector of the vehicle as $\zeta ={[Y,\beta ,\phi ,\dot{\phi }]}^T$ and the control vector $u=\left[{\delta }_f\right]$, the nonlinear dynamics model of the vehicle can be obtained by Equation (6):

$$\dot{\zeta }(t)=f(\zeta (t),u(t))$$

(6)

2.1.2. Obstacle Vehicle Model

In order to ensure safe obstacle avoidance, it is necessary to consider both the current position and the future motion trend of the dynamic obstacles. Previous studies have extensively investigated the measurement of coordinates, yaw angle and yaw rate of obstacles [20,21]. In this paper, based on the premise that the obstacle parameters mentioned above are obtainable at each sampling moment, the motion model of an obstacle vehicle in the MPC prediction horizon is modeled with the following equations:

$$\{\begin{array}{c}{X}_{\mathrm{obs}}\left(s+1\right)=X_{\mathrm{obs}}\left(s\right)+T_S{V}_{obs}\left(s\right)\cos \left({\phi }_{obs}\left(s\right)\right)\\ Y_{\mathrm{obs}}\left(s+1\right)=Y_{\mathrm{obs}}\left(s\right)+T_S{V}_{obs}\left(s\right)\sin \left({\phi }_{obs}\left(s\right)\right)\\ {\phi }_{obs}\left(s+1\right)={\phi }_{obs}\left(s\right)+T_S{\dot{\phi }}_{obs}\left(s\right)\end{array}$$

(7)

where $X_{\mathrm{obs}}\left(s\right)$ and $Y_{\mathrm{obs}}\left(s\right)$ represent the longitudinal and lateral coordinates of the obstacle vehicle in the geodetic coordinate system at moment s. $V_{obs}\left(k\right)$, ${\phi }_{obs}\left(s\right)$ and ${\dot{\phi }}_{obs}\left(s\right)$ are the current speed, yaw angle and yaw rate, respectively, and $T_S$ is the sampling interval.

2.2. GPR-Based Obstacle Avoidance Safety Envelope

Nonlinear safety avoidance envelopes are produced with a GPR model whose inputs are the main feature parameters of road conditions and obstacle states. Subsequently, the model was trained with safety avoidance data for human drivers collected through a driving simulator.

A Gaussian Process is a stochastic process specified by its mean function and covariance function [14,22]. Essentially, a GP is a set of random variables and any finite number of random variables have a joint Gaussian distribution [23,24]. With GP regressions, the mapping relationship between the input feature vector $f$ and the Gaussian distribution of the output $d$ is established by the GPR model, which can be expressed mathematically as follows:

$$d(f)\sim \mathcal{G}\mathcal{P}\left(m\left(f\right),K\left(f,f^{\prime }\right)\right)$$

(8)

where $m\left(f\right)$ and $K\left(f,f^{\prime }\right)$ represent the mean function and covariance function, respectively:

$$m\left(f\right)={\rm E}\left[d\left(f\right)\right]$$

(9)

$$K\left(f,f^{\prime }\right)={\rm E}\left[\left(d\left(f\right)-m\left(f\right)\right)\left(d\left(f^{\prime }\right)-m\left(f^{\prime }\right)\right)\right]$$

(10)

The $E$ represents mathematical expectation. Assume the mean functions are zero and adopt the square exponential kernel function shown in Equation (11) as the covariance function to obtain the prior distribution of the sample:

$$K\left(f,f^{\prime }\right)={{\sigma }_f}^2\exp \left(-\frac{1}{2}{\left(f-f^{\prime }\right)}^T{M}^{-1}\left(f-f^{\prime }\right)\right)-{{\sigma }_n}^2$$

(11)

where $M=diag\left(\left[\begin{array}{ccc}{l}_1& l_2& l_3\end{array}\right]\right)$ represents the weight matrix consisting of feature length scales for each dimension of the input vector. ${{\sigma }_f}^2$ and ${{\sigma }_n}^2$ represent the process standard deviation and the noise variance, respectively. $M$, ${{\sigma }_f}^2$ and ${{\sigma }_n}^2$ are hyperparameters and are represented by the parameter vector $h=\left[M,{{\sigma }_f}^2,{{\sigma }_n}^2\right]$. The hyperparameters have a significant impact on the performance of the GPR model [25]. In this paper, given a training set containing N inputs and corresponding outputs, hyperparameters are obtained with the maximum log-likelihood estimation as follows:

$$\mathcal{D}=\left\{F=\left[f_1,f_2,\cdots ,f_N\right],Y=\left[d_1,d_2,\cdots ,d_N\right]\right\}$$

(12)

$$h^{\ast }=\arg \max \left(\log p\left(Y|F,h\right)\right)$$

(13)

where

$$\{\begin{array}{c}\log p\left(Y|F,h\right)=-\frac{1}{2}{Y}^T{K}_{Y}Y-\frac{1}{2}\log \left|K_Y\right|-\frac{N}{2}\log \left(2\pi \right)\\ \begin{array}{c}{K}_Y=K\left(F,F\right)+{{\sigma }_n}^{2}I\\ {\left[K\left(F,F\right)\right]}_{ij}=K(f_i,f_j)\end{array}\end{array}$$

(14)

where $I$ represents the unit matrix. After obtaining the optimal set of hyperparameters and updating the kernel function, for the new prediction points $f^{\ast }$, the predicted values $d(f^{\ast })$ and the training set output Y should satisfy the joint Gaussian distribution as follows:

$$\left(\begin{array}{c}Y\\ d(f^{\ast })\end{array}\right)\sim \mathcal{N}\left(\left(\begin{array}{c}m\left(F\right)\\ m\left(f^{\ast }\right)\end{array}\right),\left(\begin{array}{cc}K\left(F,F\right)+{{\sigma }_n}^{2}I& K\left(F,f^{\ast }\right)\\ K\left(f^{\ast },F\right)& K\left(f^{\ast },f^{\ast }\right)\end{array}\right)\right)$$

(15)

Furthermore, the posterior distribution of the predicted values $d(f^{\ast })$ is also a Gaussian distribution with mean and variance.

$$d(f^{\ast })\sim \mathcal{N}(\frac{K(f^{\ast },F)\cdot Y}{K(F,F)+{\sigma }_n^{2}I},k(f^{\ast },f^{\ast })-\frac{K(f^{\ast },F)\cdot K(F,f^{\ast })}{K(F,F)+{\sigma }_n^{2}I})$$

(16)

As shown in Figure 3, in order to consider the size of the obstacle and the relative position between the obstacle and the road boundary, in this paper we adopt the concept of “channel tubes”, used in the literature [12,26] to link the infeasible gaps between the obstacles and the road boundaries, unifying the representation of the obstacles through the uncrossable area.

Further, the farthest distance $L$ between the ego-vehicle and the uncrossable area and the width $W$ of the uncrossable area are selected as the feature parameters. Considering the difference of obstacle avoidance behaviors under different vehicle speeds, the feature vector $\mathrm{f}=\left[L,W,V\right]$ is adopted as the input of the GPR model. In order to unify the probability distribution of obstacle avoidance trajectory points regardless of the lane that the ego-vehicle is in, the relative distance $d$ between the ego-vehicle and the boundary of the starting lane of obstacle avoidance is adopted as the output of the model.

The training data of the GPR model is collected using a motion-base driving simulator consisting of a real vehicle cockpit and a 180° ring screen as shown in Figure 4. A standard two-lane road is constructed as shown in Figure 5, with obstacles in the left and right lanes at intervals with a longitudinal distance (120–150) m. Drivers with extensive driving experience are recruited to drive the vehicle to complete the obstacle avoidance experiments. Drivers are instructed to perform obstacle avoidance operations at diverse safe distances based on their driving experiences. To consider the impact of driving speed on the process of obstacle avoidance without increasing the operational difficulty, we set up three groups of experiments; the speed intervals of each group are (0–30 km/h), (30–50 km/h) and (50–70 km/h), respectively. In each group of obstacle avoidance experiments, drivers can accelerate and decelerate appropriately within the speed interval according to their driving habits.

The data obtained by collecting samples are expressed in the form of data pairs:

$$D_{dyn}=\left\{f_k,d_k\right\}=\left\{L_k,W_k,V_k,d_k\right\},\forall k\in \left\{0,1,\cdots ,N\right\}$$

(17)

The size of the GPR data dictionary has a significant impact on the performance of the model [27]. As the dictionary data increases, the accuracy of the GPR model is improved, but the computational burden also increases significantly, often causing failures of model convergence. In this study, the maximum size of the GPR data dictionary is set to 1000 empirically. Once the training data size reaches the maximum, referring to the literature [28], we evaluate the new data using distance measures to determine which data samples are substituted with new ones. This distance measure is defined as the posterior variance of the new data $f^{\ast }$ points as follows:

$${\Theta }_{\ast }=K_{f\ast ,f\ast }-K_{f\ast ,F\ast }{\left(K_{F\ast ,F\ast }+\sigma I\right)}^{-1}{K}_{F\ast ,f\ast }$$

(18)

where $\sigma$ is the optimization adjustment parameter and $K_{F\ast ,F\ast }$ represents the covariance matrix. $F\ast$ is the dataset in the dictionary. The new data is evaluated by feedback gain and the data point with the smallest measurement distance will be replaced.

2.3. Envelope-Based Obstacle Avoidance Tracking Controller

Based on the obstacle avoidance safety envelope produced by the GPR model, a novel MPC control scheme is proposed. The obstacle avoidance safety envelope is transformed into soft and hard constraints. Both optimizing vehicle stability and steering smoothness are formulated into the objective functions to realize stable and smooth control of the vehicle.

To improve the computational efficiency and real-time performance of the controller, the vehicle dynamics model established in Section 2 is discretized with a fixed sampling time $T_S$ and Taylor expansion at the operating point, the following expression being obtained:

$$\zeta (s+1)=A_s\zeta (s)+B_{s}u(s)$$

(19)

$$Y(s)=H_{\mathrm{s}}\zeta (s)$$

(20)

$$\eta (s)=C_s\zeta (s)$$

(21)

where

$$A_s=\left[\begin{array}{cccc}1& T_S{v}_x\cos \phi & T_S(v_x\cos \phi -v_y\sin \phi )& 0\\ 0& 1-\frac{2T_S(C_{cf}+C_{cr})}{m{v}_x}& 0& \frac{2T_S(C_{cr}{L}_r-C_{cf}{L}_f)}{m{v}_x^2}-T_S\\ 0& 0& 1& T_S\\ 0& \frac{2T_S(C_{cr}{L}_r-C_{cf}{L}_f)}{I_z}& 0& 1-\frac{2T_S(C_{cr}{L}_r^2+C_{cf}{L}_f^2)}{I_z{v}_x}\end{array}\right]$$

$$B_s={\left[0,\frac{2T_S{C}_{cf}}{m{v}_x},0,\frac{2T_S{C}_{cf}{l}_f}{I_z}\right]}^T,C_s=\left[\begin{array}{cccc}1& 0& 0& 0\end{array}\right],H_s=\left[\begin{array}{cccc}0& 1& 0& 0\\ 0& 0& 0& 1\end{array}\right]$$

Adding the control factors $u(s-1)$ to $\xi \left(s\right)$, a new state quantity $\tilde{\xi }\left(s\right)$ is given by the following equations.

$$\tilde{\zeta }(s+1)={\tilde{A}}_s\tilde{\zeta }(s)+{\tilde{B}}_s\Delta u(s)$$

(22)

$$Y(s)={\tilde{H}}_s\zeta (s)$$

(23)

$$\eta (s)={\tilde{C}}_s\tilde{\zeta }(s)$$

(24)

where

$$\tilde{\zeta }(s)={\left[\begin{array}{ccccc}Y(s)& \beta (s)& \phi (s)& \dot{\phi }(s)& {\delta }_f(s-1)\end{array}\right]}^T,\Delta u\left(s\right)=u(s)-u(s-1)$$

$${\tilde{A}}_s=\left[\begin{array}{cc}{A}_s& B_s\\ 0& I\end{array}\right],{\tilde{B}}_s={\left[\begin{array}{cc}{B}_s& I\end{array}\right]}^T,{\tilde{H}}_s=\left[\begin{array}{cc}{H}_s& 0_{2\times 1}\end{array}\right],{\tilde{C}}_s=\left[\begin{array}{cc}{C}_s& 0\end{array}\right]$$

Assuming that the vehicle speed remains constant in the predicted horizon, as shown in Figure 6, based on the obstacle vehicle model constructed in Section 2, at each sampling moment the feature input of the GPR model at the next $P_N$ prediction points can be obtained.

$$\begin{array}{l}{F}_{P_N}=\left\{\begin{array}{cccc}{f}_{k+1}& f_{k+2}& \cdots & f_{k+P_N}\end{array}\right\}\\ =\left\{\begin{array}{cccc}\left\{L_{k+1},W_{k+1},v_x(k)\right\}& \left\{L_{k+2},W_{k+2},v_x(k)\right\}& \cdots & \left\{L_{k+P_N},W_{k+P_N},v_x(k)\right\}\end{array}\right\}\end{array}$$

(25)

Inputting $F_{P_N}$ into the GPR model to obtain a Gaussian distribution corresponding to the future N predicted outputs and transforming them into the global coordinate system:

$$Y(k+i)~\mathcal{N}\left(m_{k+i},{\sigma }_{k+i}\right),i=1,2,\cdots ,P_N$$

(26)

Referring to [29], when the obstacle avoidance trajectory point is located in a small probability interval with high uncertainty, it is identified as having a collision risk. Therefore, two confidence intervals $(m_{k+i}\pm {\sigma }_{k+i})$ and $(m_{k+i}\pm 2{\sigma }_{k+i})$ are adopted as the soft and hard constraints to ensure driving safety and vehicle stability. The hard constraint ensures that the obstacle avoidance trajectory of the vehicle is collision-free. The soft constraint ensures that the vehicle drives along the center of the safety region when possible while allowing the vehicle to go beyond the soft intervals to maintain vehicle stability. Assuming that the current time step is k, the vehicle control increment remains constant between the control horizon and the prediction horizon, and the vehicle states in the state matrix have been obtained from the on-board sensors and the state observer, the designed multi-objective MPC is expressed as in Equations (27)–(32). The stability feature parameters sideslip angle $\beta$ and yaw rate $\dot{\phi }$ are selected as outputs.

$$\underset{\Delta U(k)}{\min }{\begin{array}{c}{J}_k={\displaystyle \sum _{i=1}^{P_N}{\Vert \eta \left(k+i\right)\Vert }^2}\end{array}}_Q+{\displaystyle \sum _{i=0}^{C_N-1}{{\Vert \Delta u\left(k+i\right)\Vert }^2}_R}+\rho {\epsilon }^2$$

(27)

subj. to Equations (28)–(32)

$$\begin{array}{c}\tilde{\zeta }(k+1)={\tilde{A}}_k\tilde{\zeta }(k)+{\tilde{B}}_k\Delta u(k)\\ Y(k)={\tilde{H}}_k\zeta (k)\\ \eta (k)={\tilde{C}}_k\tilde{\zeta }(k)\end{array}$$

(28)

$$\Delta u(k)=u(k)-u(k-1)$$

(29)

$$m_{k+i}-\left(1+\epsilon \right){\sigma }_{k+i}\le Y\left(k+i\right)\le m_{k+i}+\left(1+\epsilon \right){\sigma }_{k+i}$$

(30)

$$0\le \epsilon \le 1$$

(31)

$$\begin{array}{l}{u}_{\min }\le u(k)\le u_{\max }\\ \Delta u_{\min }\le \Delta u(k)\le \Delta u_{\max }\end{array}$$

(32)

where $Q$, $R$ and $\rho$ are the weight matrices of output, input and slack variables respectively. Equations (31) and (32) represent the constraints on the slack factor, control input and control increment, respectively.

It is assumed that the control input remains constant during each control step. Transform Equation (27) into standard quadratic form combined with the constraints:

$$\underset{\Delta U(k),\epsilon }{\min }\frac{1}{2}{\left[\begin{array}{c}\Delta U(k)\\ \epsilon \end{array}\right]}^T{P}_e\left[\begin{array}{c}\Delta U(k)\\ \epsilon \end{array}\right]+G_e\left[\begin{array}{c}\Delta U(k)\\ \epsilon \end{array}\right]$$

(33)

subj. to

$$\left[\begin{array}{cc}I& 0\\ -I& 0\\ ℝ& 0\\ -ℝ& 0\\ 0& 1\\ 0& -1\\ {\Theta }_{\mathrm{k}}& -{\Omega }_k\\ -{\Theta }_k& -{\Omega }_k\end{array}\right]\left[\begin{array}{c}\Delta U(k)\\ \epsilon \end{array}\right]\le \left[\begin{array}{c}\Delta {\delta }_{f,\max }\times ones\left(C_N,1\right)\\ -\Delta {\delta }_{f,\min }\times ones\left(C_N,1\right)\\ {\delta }_{f,\max }\times ones\left(C_N,1\right)\\ -{\delta }_{f,\min }\times ones\left(C_N,1\right)\\ 1\\ 0\\ M_k+{\Omega }_k-{\Psi }_k\tilde{\zeta }\left(k\right)\\ -M_k+{\Omega }_k+{\Psi }_k\tilde{\zeta }\left(k\right)\end{array}\right]$$

(34)

where

$$\begin{array}{l}\begin{array}{cc}{\Omega }_k={\left[\begin{array}{cccc}{\sigma }_{k+1}& {\sigma }_{k+2}& \cdots & {\sigma }_{k+P_N}\end{array}\right]}^T,& {\mathsf{{\rm M}}}_k={\left[\begin{array}{cccc}{m}_{k+1}& m_{k+2}& \cdots & m_{k+P_N}\end{array}\right]}^T\end{array}\\ \Delta U\left(k\right)={\left[\begin{array}{cccc}\Delta u\left(k\right)& \Delta u\left(k+1\right)& \cdots & \Delta u\left(k+C_N-1\right)\end{array}\right]}^T,\begin{array}{cc}{Q}_e=I_{P_N}\otimes Q,& R_e=I_{C_N}\otimes R\end{array}\\ \begin{array}{cc}{P}_e=\left[\begin{array}{cc}2\left({{\Xi }_k}^T{Q}_e{\Xi }_k+R_e\right)& 0_{C_N\times 1}\\ 0_{1\times c_N}& \rho \end{array}\right]& G_e\end{array}=\left[\begin{array}{cc}2\tilde{\zeta }{\left(k\right)}^T{{\Gamma }_k}^T{Q}_e{\Xi }_k& 0\end{array}\right]\end{array}$$

$${\Xi }_k={\left[\begin{array}{cccc}{\tilde{C}}_k{\tilde{B}}_K& 0& \cdots & 0\\ {\tilde{C}}_k{\tilde{A}}_k{\tilde{B}}_K& {\tilde{C}}_k{\tilde{B}}_{k+1}& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ {\tilde{C}}_k{\displaystyle \prod _{i=0}^{P_N-1}{\tilde{A}}_{k+i}{\tilde{B}}_k}& {\tilde{C}}_k{\displaystyle \prod _{i=1}^{P_N-1}{\tilde{A}}_{k+i}{\tilde{B}}_{k+1}}& \cdots & {\tilde{C}}_k{\displaystyle \prod _{i=C_N-1}^{P_N-1}{\tilde{A}}_{k+i}{\tilde{B}}_{k+C_N-1}}\end{array}\right]}_{P_N\times C_N}$$

$$\begin{array}{cc}ℝ={\left[\begin{array}{cccc}1& 0& \cdots & 0\\ 1& 1& \cdots & 0\\ ⋮& ⋮& \ddots & 0\\ 1& 1& \cdots & 1\end{array}\right]}_{C_N\times C_N}& {\Gamma }_k={\left[\begin{array}{c}{\tilde{C}}_k{\tilde{A}}_k\\ {\tilde{C}}_k{\tilde{A}}_k{\tilde{A}}_{k+1}\\ ⋮\\ {\tilde{C}}_k{\displaystyle \prod _{i=0}^{P_N-1}{\tilde{A}}_{k+i}}\end{array}\right]}_{P_N\times 1}\end{array}$$

$${\Theta }_{\mathrm{k}}\begin{array}{cc}={\left[\begin{array}{cccc}{\tilde{H}}_k{\tilde{B}}_K& 0& \cdots & 0\\ {\tilde{H}}_k{\tilde{A}}_k{\tilde{B}}_K& {\tilde{H}}_k{\tilde{B}}_{k+1}& \cdots & 0\\ ⋮& ⋮& \ddots & ⋮\\ {\tilde{H}}_k{\displaystyle \prod _{i=0}^{P_N-1}{\tilde{A}}_{k+i}{\tilde{B}}_k}& {\tilde{H}}_k{\displaystyle \prod _{i=1}^{P_N-1}{\tilde{A}}_{k+i}{\tilde{B}}_{k+1}}& \cdots & {\tilde{H}}_k{\displaystyle \prod _{i=C_N-1}^{P_N-1}{\tilde{A}}_{k+i}{\tilde{B}}_{k+C_N-1}}\end{array}\right]}_{P_N\times C_N}& {\Psi }_k\end{array}={\left[\begin{array}{c}{\tilde{H}}_k{\tilde{A}}_k\\ {\tilde{H}}_k{\tilde{A}}_k{\tilde{A}}_{k+1}\\ ⋮\\ {\tilde{H}}_k{\displaystyle \prod _{i=0}^{P_N-1}{\tilde{A}}_{k+i}}\end{array}\right]}_{P_N\times 1}$$

By solving Equation (33), the sequence of control inputs at each step is obtained and the first element is chosen as the control variable. As a result, the trajectory that satisfies the objectives of optimal vehicle stability and steering smoothness within the safe obstacle avoidance region is executed:

$${\delta }_f\left(k\right)={\delta }_f\left(k-1\right)+\Delta {{\delta }_{f,k}}^{\ast }$$

(35)

3. Experiments

To verify the proposed obstacle avoidance strategy, a series of obstacle avoidance simulation studies are conducted using the motion-base driving simulator described in Section 2.2. The vehicle dynamics are simulated using the CarSim B-class sedan model, which outputs the vehicle states at a frequency of 50 Hz. The driving simulation program provides a MATLAB/Simulink interface which receives real-time control commands generated by the proposed obstacle avoidance controller. The parameters of the vehicle dynamics model and the MPC controller are presented in

Table 2. Parameters of the vehicle model.

Symbol	Value
m	1723 kg
$I_Z$	4175 kg⋅${\mathrm{m}}^2$
$L_f$	1230 mm
$L_r$	1470 mm
$C_{cr}$	62,700 N⋅${\mathrm{rad}}^{-1}$
$C_{cf}$	66,900 N⋅${\mathrm{rad}}^{-1}$

and

Table 3. Parameters of the MPC controller.

Symbol	Value
T	0.02
${\mathrm{P}}_N$	20
$C_N$	5
Q	$\left[\begin{array}{cc}10000& 0\\ 0& 2000\end{array}\right]$
R	50000
$\epsilon$	1000

.

3.1. Scenario A

Scenario A is a standard two-lane continuous obstacle avoidance scenario with lane width of 3.5 m, where three static obstacle vehicles of 4.65 m length and 2.1 m width are set at positions (99, 2.75), (190, 6.25) and (295, 2.75), respectively. In order to verify the effectiveness of the proposed MPC controller based on the GPR safe obstacle avoidance envelope, in ensuring vehicle stability and control smoothness while performing safe obstacle avoidance, another MPC controller is built as a comparison. The comparison controller uses the mean value of the GPR real-time output as the reference trajectory and the objective functions and weight coefficients are defined referring to the literature [30]. The proposed obstacle avoidance controller in this paper is denoted as controller A and the controller for comparison is denoted as controller B. A fine driving condition with a high ground adhesion coefficient (μ = 0.85) and a hazardous driving condition with a low adhesion coefficient (μ = 0.2) are used to compare the tracking performance of the controllers and the vehicle speed was set at a constant 72 km/h. The experimental results are shown in Figure 7 and Figure 8, respectively.

Figure 7 illustrates the experimental results of obstacle avoidance on the high-adhesion road. As shown in Figure 7a, both controller A and controller B achieve safe obstacle avoidance. Since no specific reference trajectory is given, controller A would start the obstacle avoidance operation in advance when facing each obstacle and choose the smoothest obstacle avoidance route possible within the soft constraints $\left(\mu \pm \sigma \right)$ to ensure high level of safety, as well as optimal stability and control smoothness. As shown in Figure 7b, the front wheel steering angles of controller A and controller B both stay within a reasonable range throughout the obstacle avoidance process and the front wheel steering angle of controller B is significantly greater than that of controller A, with a maximum steering angle of 1.55 deg compared to 1.1 deg for controller A. It is also shown in Figure 7c,d that, throughout the obstacle avoidance process, the control increment of controller A is smoother and the slip angle and yaw rate of controller A are both smaller than those of controller B, demonstrating better vehicle stability.

When the vehicle is driven under hazardous conditions with low adhesion, as shown in Figure 8, controller B yields significant tracking errors due to handling limits (Figure 8a). To eliminate the tracking error, the steering control command of controller B increases rapidly to a maximum of more than 4 deg. Meanwhile, the yaw rate and sideslip angle of controller B increase significantly, exceeding 0.3 rad/s and 1.1 deg, respectively, indicating that the vehicle has already become instable in attempting to track the reference trajectory. By contrast, the trajectory of controller A exceeds the interval $\left(\mu \pm \sigma \right)$, but is still located within $\left(\mu \pm 2\sigma \right)$, indicating that the vehicle exceeds the soft constraint in order to maintain stability, but still ensures safe driving without collision. As presented in Figure 8b–d, the steering angle, yaw rate and sideslip angle of controller A are much smaller than those of controller B, proving the capability of controller A in maintaining a good level of stability while safely avoiding the obstacles, even under challenging conditions.

The results presented above clearly show that the proposed MPC tracking controller based on the safety avoidance envelope can make full use of the vehicle’s handling limit to ensure stable and smooth driving under the premise of safe obstacle avoidance.

3.2. Scenario B

In scenario B, to verify the effectiveness of the proposed obstacle avoidance control scheme in more complex situations where both dynamic and static vehicles are presented, we set a dynamic vehicle with a speed of 10 m/s in the first lane and a static obstacle is placed at position (180, 2.75) in the second lane. The ego-vehicle is driving in the first lane at a speed of 20 m/s. To avoid collision with the dynamic vehicle in the first lane, the vehicle needs to change to the second lane for overtaking and then return to the original lane to avoid the static vehicle.

Figure 9 shows the experimental results of scenario B. As shown in Figure 9a,b, the obstacle avoidance trajectory of the ego-vehicle is always located within the interval $\left(\mu \pm \sigma \right)$ and the minimum distances between the ego-vehicle and the dynamic and static obstacle are 3.44 m and 2.99 m, respectively, both greater than the safety distance of 2.1 m, considering the length of the vehicles. As shown in Figure 9c,e,f, during the entire process, the front steering angle of the vehicle is quite smooth, which ensures the smoothness and comfort of the obstacle avoidance process. The maximum yaw rate and the sideslip angle were no more than 0.07 rad/s and 0.08 deg, respectively, indicating that the vehicle is maintained at a stable state.

The results presented verify that the data–driven GPR model proposed in this paper can accurately generate nonlinear safety envelopes under complex scenarios with dynamic obstacles.

3.3. Scenario C

In scenario C, to verify the effectiveness of the proposed obstacle avoidance scheme in coping with environmental uncertainty through the prediction of obstacle motion trends in the MPC prediction horizon, we set up a standard three-lane continuous obstacle avoidance scenario with a lane width of 3.5 m. As shown in Figure 10a, from the top down, the first lane, second lane and third lane are shown, respectively. The ego-vehicle starts at (0, 6.2) and drives in the second lane at a speed of 20 m/s. In the third lane, a dynamic obstacle vehicle changes lanes towards the second lane at a speed of 10 m/s and a static obstacle vehicle is placed at position (160, 9.75) in the first lane. There will be a collision between the two vehicles if both the ego-vehicle and the obstacle vehicle drive in their current state. To avoid collisions, the ego-vehicle needs to change to the first lane and, after the overtaking is completed, the ego-vehicle needs to return to the second lane to avoid the static obstacle.

Figure 10 shows the experimental results of scenario C. As shown in Figure 10a,b, based on the constructed obstacle vehicle model, the ego-vehicle enables active and safe obstacle avoidance when the obstacle vehicle merges in, and the minimum distances between the ego-vehicle and the dynamic and static obstacle are 3.19 m and 3.45 m, respectively, both reasonable safe distances. Although there are some abrupt changes in the generated GPR envelope due to sudden changes in the motions of the dynamic vehicle, the proposed controller ensures smooth obstacle avoidance by crossing the soft constraint and avoids collision by staying within the hard constraint $\left(\mu \pm 2\sigma \right)$. As shown in Figure 10d,e, during the entire process, the maximum yaw rate and the sideslip angle were no more than 0.08 rad/s and 0.18 deg, respectively, indicating that the vehicle is maintained at a stable state. The experimental results prove that the obstacle avoidance controller is robust to environmental uncertainties in the sense that the GPR model is capable of generating safety envelopes in response to abrupt changes in the motion states of surrounding obstacles. Furthermore the proposed constrained multi-objective MPC controller is able to perform safe and stable obstacle avoidance based on the dynamically generated envelopes.

4. Conclusions

This paper presents an active obstacle avoidance framework for collision-free obstacle avoidance while ensuring vehicle stability and steering smoothness. First, scene-adaptive safety envelopes representing the safety boundaries of obstacle avoidance are generated with a GPR model from observed road conditions and the states of obstacles. On the basis of the safety envelopes, soft and hard constraints are generated and incorporated into the MPC controller, and are rolling updated in the prediction horizon. Both minimizing the control increments and stability feature parameters are formulated into the objectives of the MPC controller. By solving the constrained multi-objective optimization problem, control commands that guarantee safe obstacle avoidance with complex vehicle dynamics taken into account are obtained. In contrast to hierarchical and sequential methodologies consisting of separated path planning and trajectory tracking procedures, the proposed framework solves the dilemma of tracking infeasible paths by modeling the procedure of obstacle avoidance as an integrated constrained multi-objective optimization problem. A series of experiments are conducted on a motion-base driving simulator to verify the effectiveness of the proposed framework under various conditions. The results prove that the data-driven GPR model proposed in this paper is highly adaptable to complex obstacle avoidance scenarios and can accurately and robustly produce safety envelopes in various scenes. Consequently, the proposed MPC controller built upon the safety envelopes is robust and adaptive to challenging conditions, demonstrating optimal performance in avoiding obstacles safely, with vehicle stability guaranteed.