Driving and steering collision avoidance system of autonomous vehicle with model predictive control based on non-convex optimization

Abstract

A planar motion control system is proposed for autonomous vehicles not only to follow the lanes, but also to avoid collisions by braking, accelerating, and steering. The supervisor is designed first to determine the desired speed and the risk of the maneuvering due to road boundaries and obstacles. In order to allow lane changes on multi-lane roads, the model predictive controller is formulated based on the probabilistic non-convex optimization. The micro-genetic algorithm is applied to calculate the target speed and target steering angle in real time. A software-in-the-loop unit is constructed with the Rapid Control Prototyping device in the vehicle communication environment. The performance of the proposed system is verified for various collision avoidance scenarios and the simulation results demonstrate the safe and effective driving performance of autonomous vehicles with no collision on multi-lane road.

Introduction

Autonomous vehicles have been proposed as a solution to problems related to car-to-car accidents due to increased traffic volume. ¹ According to recent surveys, car-to-car crashes are the most frequent type of traffic accidents, requiring active safety technology and self-driving technology. ² There is a growing demand for an autonomous vehicle system that covers the complicated situations such as lane change and collision avoidance. Even if many studies on path planning and control for Collision Avoidance (CA) have been conducted,^3–15 these technologies are still in the development stage such as the complicated supervisor design including the risk assessment and CA controller design. Risk assessment is a method of determining the risk level of the object quantitatively. Collision avoidance is an operation that is initiated after the imminent collision is detected from the risk assessment.

The risk level assessment of the surrounding objects is very critical for the successful collision avoidance. In many risk assessment studies, the Artificial Potential Field (APF) method has been often utilized.^{3,9,12–14,16} APF is based on an energy field to express physical force or risk probability for obstacle avoidance in robotics since 1980.^16–19 Recently, the APF concept is applied for optimal path planning and control in the automotive field.^7,9,12,13,15 For instance, Ji et al. ¹² proposed a trajectory planning in the optimal gradient direction of the potential field. This method can calculate the route quickly, but, there is a possibility that the calculated path does not consider the dynamic characteristics of the vehicles. Rasekhipour et al. ¹³ defines two kinds of the obstacle APF: crossable and non-crossable object APF based on the amplitude of the APF. Because they utilized the linear programming solver for approximating the nonlinear objective function, the distortion of the risk assessment is inherent. In addition, geometry-based risk functions have been suggested for the CA problem.^6,7,20,21 Werling and Liccardo ⁶ formulated the risk assessment by taking into account the minimum distance to the object. The advantage of this method is that it quickly determines collision-free maneuvering by predicting the future risk with the vehicle model. Rosolia et al. ²⁰ defined the collision risk with oval which covers entire vehicle. Since the collision risk is differentiable, the oval collision risk is suitable to linear programming solver. Li et al. ²¹ decomposed the vehicle shape by circles which cover the vehicle entirely and checked the distance between the circles on the ego vehicle and the circles on the obstacle vehicle. The advantage of these methods is that the collision risk can be easily checked by simple approximation. But, these methods assume simple vehicle shape such that the shape of risk can be different from the actual vehicle.

Among the various CA controllers, model predictive control is the most actively utilized.^6,7,13,15 Its ability to handle the actuator capacity and the output constraint is crucial despite the burden of computation time. In particular, unlike the conventional control techniques, it is possible to design specific and descriptive scenarios using MPC conditions. Rasekhipour et al. ¹³ suggested Sequential Quadratic Programming (SQP) based MPC to calculate steering angles and target speeds considering the APF risk model. However, in order to implement QP MPC control, the objective function needs to be a convex function and the constraint should satisfy the convex set. ²² In actual road conditions, however, situations may exist that cannot be easily described by convex optimization. For example, as shown in Figure 1, path A has wider free space than path B and the ego-vehicle (blue) can avoid collision more safely by taking path A if the next lane is empty. However, path B would be the CA result of convex optimization because the ego-vehicle is driven on the second lane before the CA action and path B is relatively closer. In addition, convex optimization can consider the action near the local minimum and the determined action cannot be changed to another optimal action. Thus, in theQP-based optimal controller, it may be difficult to use the non-convex functions as the risk function.

Figure 1.

Planar control problem that convex optimization does not solve well.

To process the non-convex formulation without approximation, an optimization tool is needed to find the global optimal point for the CA problem. Among the candidate solutions, there exist gradient-free search methods such as genetic algorithm, particle swarm algorithm, and pattern search algorithm.^23–26 However, many of the global optimization methods are not proper for real-time control task of autonomous vehicles, since these methods have high computational complexity as mentioned in Fleming and Fonseca ²⁷ and Wang et al. ²⁸ Because of the difficulty in archiving real-time performance, other techniques have been investigated with multicore CPU.^28,29 While the above global optimization techniques involve significant computational burden, the CA with micro-genetic algorithm ( $\mu$ -GA) is suggested by Son ¹⁴ to guarantee real-time performance by using a small population. They designed a collision avoidance controller using $\mu$ -GA and MPC to find the global optimum solution quickly. But, they did not consider risk functions of various driving environment and multi-modal nature of the problem.

In this study, an autonomous driving system for normal driving and collision avoidance is proposed based on nonconvex optimization and the MPC. At first, a supervisor is described to predict the collision risk and desired speed. The risk functions of road boundary and vehicles are modeled using different mathematical functions. In particular, the Gilbert-Johnson-Keerthi (GJK) algorithm ³⁰ is simplified to detect the obstacle collision with less computational complexity. Also, the motion of the surrounding vehicles and the desired speed of the host vehicle are predicted along the road geometry. Considering the planar motion of the host vehicle, a non-convex optimization problem is defined for the Gaussian objective function of the collision risk and reference input. Finally, the optimal problem is solved by using the modified $\mu$ -GA. The proposed system is verified by using co-simulation of commercial software and RCP (Rapid Control Prototyping) device. The main contribution of the proposed study is summarized as follows:

• -Multi-reference control formulation is proposed based on the non-convex optimization such as speed tracking, lane keeping, and collision avoidance by decelerating or changing lanes for autonomous driving

• -The GJK algorithm is simplified for less computation burden and utilized to assess the collision risk between vehicles

• -The $\mu$ -GA solver is modified for real-time computation and its performance is verified on CAN network with a RCP device.

Planar motion control system configuration

The planar motion of the autonomous vehicle is determined by two inputs: steering angle and longitudinal speed. It is assumed that the autonomous vehicle is equipped with a speed control module such as Adaptive Cruise Control (ACC). The surrounding information such as lane markers, vehicles, and obstacles is also assumed available through in-vehicle sensors. The lane information is assumed to be provided in the form of a cubic polynomial with respect to the host vehicle. ³¹ The practical issues such as sensor uncertainty are not considered in this formulation. In addition, the motion data of the vehicle is assumed to be available either by vehicle network or by estimation.

The block diagram in Figure 2 shows the structure of the planar motion control (PMC) system. The proposed PMC system is composed of supervisor and MPC controller. The purpose of the supervisor is to obtain the collision risk and to determine the maximum speed for the given road environment. The input to the supervisor is the information of the road and the surrounding vehicles. The supervisor is constructed by utilizing the risk models along the predicted trajectory of the host vehicle and provides the collision risk on a geometric basis and the desired speed of the host vehicle. The MPC is constructed based on the objective function consisting of the collision risk of the obstacles and the maneuvering command. Non-convex optimization problem is solved for the steering angle and the target speed by utilizing the $\mu$ -GA method.

Figure 2.

Planar motion control system architecture.

Supervisor design

Risk modeling for the surroundings and collision prediction

Based on the relative position of road boundaries and obstacles, the corresponding risk can be expressed in the form of a specific function. The risk of the road boundary is described as a function having the maximum value near the boundary with a shape close to the cumulative normal distribution. ³² The risk of road boundaries, $R_{\mathrm{road}}$ is defined as a function of the lateral displacement, $y$ , in the vehicle-fixed coordinate:

$$R_{\mathrm{road}}=\mathrm{sgm}(d·(y-y_{\mathrm{left}}))+1-\mathrm{sgm}(d·(y-y_{\mathrm{right}}))$$

(1)

where $y_{\mathrm{left}}$ and $y_{\mathrm{right}}$ are the lateral position of the left and right boundaries of the road, respectively. $\mathrm{sgm}$ denotes the logistic sigmoid function. ³³ The tuning parameter, $d$ , determines the steepness of the risk function and its value depends on the accuracy of the position estimation. Figure 3 visualize the risk of the road boundary with $d=5$ .

Figure 3.

Risk of 2-lane road boundary where $y_{\mathrm{left}}=5.25$ and $y_{\mathrm{right}}=-1.75$ .

In addition, predicting the behavior of the surrounding vehicles along the road is also crucial to the collision detection. The typical motion prediction model is the Constant Velocity (CV) model and there have been several studies predicting the future position of the surrounding vehicles with this model. ⁸ Another motion prediction model is Constant Turn-Rate and Acceleration (CTRA) model ³⁴ which is suitable in a curved road. However, CV model may not be suitable to describe the vehicle motion on the curved road. In addition, as shown in Figure 4, the CTRA model can not be utilized in the lane change motion prediction, in particular, for a long prediction horizon. Thus, either of these models is not suitable for predicting lane-change as well as lane centering motions.

Figure 4.

Limitation of CTRA model in the lane change motion prediction.

In order to predict the future position of the surrounding vehicles better and to describe the correlation between the vehicles and the road, the APF method is used with adjusting the vehicles motion to the descending direction of the slope of the risk function.^3,9,12 While the repulsive potential from the road boundary is considered in the road boundary risk (1), the attractive potential to the center of the lane needs to be considered. In order to include the lane change motion of the surrounding vehicles, the bounded attractive potential field is utilized instead of the unbounded one. Then, if the momentum caused by the lateral speed becomes large enough to escape the attractive force of the current lane, the surrounding vehicle is predicted to move to the adjacent lane. The attractive potential to the $i^{\mathrm{th}}$ lane, $U_{\mathrm{lane},i}$ , can be expressed as follows:

$$U_{\mathrm{lane},i}=-\exp (-0.5(y-y_{\mathrm{lane},i}{)}^2)$$

(2)

where $y_{\mathrm{lane},i}$ denotes the lateral position of the center of the $i$ th lane. Surrounding vehicles are considered as particles with 1/50,000 scale, artificial lateral dynamics (ALD) are defined. ³

$$m_{\mathrm{ob}}\stackrel{··}{y}+b_{\mathrm{ob}}\stackrel{·}{y}=-\frac{\partial }{\partial y}(\sum _{i=1}^{n_{\mathrm{lane}}}{U}_{\mathrm{lane},i}+R_{\mathrm{road}})$$

(3)

where $m_{\mathrm{ob}}$ and $b_{\mathrm{ob}}$ are the scaled mass of the vehicle and the damping coefficient of the ALD, respectively. The optimization toolbox is utilized based on the experimental data and the determined values are listed in the Appendix.

The purpose of this method is predicting the lane where surrounding vehicles would travel, rather than predicting the accurate future position of the surrounding vehicles. That is, it gives us the criterion that prevents the host vehicle being steered toward adjacent lanes where the surrounding vehicles are expected. For instance, the result of predicting cut-in vehicle trajectory is depicted in Figure 5.

Figure 5.

Trajectory prediction of the surrounding vehicle in cut-in scenario.

In order to judge collision on the predicted trajectories of the host vehicle, the collision between vehicles is interpreted in two-dimentional space and, the vehicle shape is described as a rectangle. Based on these assumptions, the collision between vehicles is simplified as the collision between two rectangles. There exist algorithms that can detect the collision between polygons.^30,35–37 Among the algorithms, GJK algorithm has low computational cost for real-time application. ³⁰ In this study, the GJK algorithm is utilized to represent the collision risk as a rectangular shape. In addition, the GJK algorithm is simplified for even less computational complexity. Conventionally, the collision risk has been typically described as circular or oval shape based on artificial potential field, but its shape includes unnecessary risk region and it can cause conservative warning and evasive maneuvering.

GJK algorithm detects collision based on the Minkowski Difference (MD) which is defined as a set of the difference between the position vectors of one polygon and the others. In case of a point-symmetric polygon, like a rectangle, it is also the same problem of whether the MD contains the origin of the surrounding vehicle as shown in Figure 6. Here, the Minkowski difference, $S_{\mathrm{Minkowski}}$ , is defined as follows:

$$S_{\mathrm{Minkowski}}=S_{\mathrm{host}}\ominus S_{\mathrm{ob}}$$

(4)

where $S_{\mathrm{host}}$ and $S_{\mathrm{ob}}$ is the sets of the vectors on the host vehicle and surrounding vehicle respectively. ⊖ denotes the MD operator.

Figure 6.

Collision detection using reduced GJK algorithm.

In order to detect the collision with the original GJK, ³⁰ it is necessary to check whether the simplex defined by the vertices of the MD contains the origin. But, this process is computationally burdensome, and thus, in this study, MD is replaced with $S_m$ which is a rectangular area enclosing the MD as shown in Figure 6 and is defined below :

$$S_m=\{x,y|\left|x\right|\le a_m+0.5l_{\mathrm{host}},\left|y\right|\le b_m+0.5w_{\mathrm{host}}\}$$

(5)

where $l_{\mathrm{host}}$ and $w_{\mathrm{host}}$ are overall length and width of the host vehicle, respectively. $x$ and $y$ are the vehicle fixed coordinate. The original GJK algorithm is simplified to detect the collisions between rectangles and the center of $S_m$ is located at the vehicle center as illustrated in Figure 6. Also, $a_m$ and $b_m$ are defined as follows:

$$\begin{array}{c}{a}_m=\frac{\sqrt{{l_{\mathrm{ob}}}^2+{w_{\mathrm{ob}}}^2}}{2}\left|\cos ({\psi }_d+\left|\mathrm{\Delta }\psi \right|)\right|,\hfill \\ \multicolumn{1}{c}{b_m=\frac{\sqrt{{l_{\mathrm{ob}}}^2+{w_{\mathrm{ob}}}^2}}{2}\left|\sin ({\psi }_d+\left|\mathrm{\Delta }\psi \right|)\right|}\end{array}$$

(6)

where $l_{\mathrm{ob}}$ and $w_{\mathrm{ob}}$ are length and width of the $S_{\mathrm{ob}}$ respectively. ${\psi }_d$ denotes the angle between the diagonal line and the heading of the surrounding vehicle. $\mathrm{\Delta }\psi$ denotes the heading difference between the host vehicle and surrounding vehicles.

Then, collision can be expected using the following condition

$$\{\begin{array}{c}\mathrm{if}(x_{\mathrm{ob}},y_{\mathrm{ob}})\in S_m,\mathrm{collision}\mathrm{or}{R}_{\mathrm{col}}=1\hfill \\ \multicolumn{1}{c}{\mathrm{else},\mathrm{no}\mathrm{collision}\mathrm{or}{R}_{\mathrm{col}}=0}\end{array}$$

(7)

Even if the simplified GJK algorithm shows less computation burden than the existing collision check algorithms, it takes longer time to check the collision risk of all the detected objects. Thus, the proposed system is designed to activate the collision detection algorithm only in necessary situations such as imminent collision. The activation threshold is set such that the object is expected to approach within 10 m radius of the host vehicle in the prediction horizon of 1 s.

Desired speed determination

Since the longitudinal dynamics affect the lateral motion of the vehicle, determining the maximum longitudinal velocity affects the performance of the PMC system. Firstly, the vehicle should follow the set speed well. Secondly, the vehicles desired speed should be adjusted depending on the curvature in cornering. For example, in order to maintain the turning radius with a constant speed, the lateral force equivalent to the centrifugal force is required. Therefore, the desired longitudinal speed at the curved road is related to the maximum lateral acceleration, $a_{y,max}$ , as follows:

$$\left|a_y\right|\le a_{y,max}$$

Assuming that the vehicle follows a path with a little side slip, the lateral acceleration can be approximated as follows:

$$a_y\approx \frac{d^{2}y}{d{x}^2}{\left(\frac{\mathrm{dx}}{\mathrm{dt}}\right)}^2,$$

and the limit of the longitudinal speed is determined. ³⁸

$$\frac{\mathrm{dx}}{\mathrm{dt}}\le \sqrt{\frac{a_{y,\max }}{\frac{d^{2}y}{d{x}^2}}}=\sqrt{\kappa a_{y,\max }}$$

(8)

where $\kappa$ is the road curvature.

Then, the desired speed is determined from the following equation where the desired speed at the curve is set to only 80% of the above speed limit for safe driving.

$$V_{x,\mathrm{des}}=\min (V_{x,\mathrm{set}},0.8\sqrt{\kappa a_{y,\max }})$$

(9)

where $V_{x,\mathrm{set}}$ and $V_{x,\mathrm{des}}$ are set speed along the road and the desired speed from the supervisor, respectively.

Non-convex model predictive controller design

In this section, a model predictive controller is formulated based on the $\mu$ -GA. The method of selecting the test points is described to obtain the real-time performance in the control input domain for steering angle and target speed.

Planar vehicle dynamics

For the simplicity of the MPC design, the following lateral dynamic model is utilized. ³⁹

$$\begin{array}{c}m(\stackrel{··}{y}+\stackrel{·}{x}\stackrel{·}{\psi })=F_{\mathrm{yf}}+F_{\mathrm{yr}}\hfill \\ \multicolumn{1}{c}{I_z\stackrel{··}{\psi }=l_f{F}_{\mathrm{yf}}-l_r{F}_{\mathrm{yr}}}\end{array}$$

(10)

where $m$ is the mass of the vehicle, $I_z$ is the inertial moment of the vehicle in the z-axis direction, and $l_f$ and $l_r$ are the distance from the center of gravity to the front and rear axles, respectively. $F_{\mathrm{yf}}$ and $F_{\mathrm{yr}}$ are lateral tire forces ate the front and rear wheels, respectively. $\psi$ is the yawing angle of the host vehicle.

As described by Jazar, ³⁹ the lateral force can be approximated by a linear function of the wheel slip angle which can be expressed based on the geometric relationships at front and rear

$$\begin{array}{c}{F}_{\mathrm{yf}}=-C_f{\alpha }_f=C_f{\delta }_f-C_f\frac{\stackrel{·}{y}+l_f\stackrel{·}{\psi }}{\stackrel{·}{x}}\hfill \\ \multicolumn{1}{c}{F_{\mathrm{yr}}=-C_r{\alpha }_r=C_r\frac{\stackrel{·}{y}-l_r\stackrel{·}{\psi }}{\stackrel{·}{x}}}\end{array}$$

(11)

where ${\delta }_f$ is the steering angle of the front wheel, and $C_f$ and $C_r$ are the cornering stiffness of the front and rear wheels. ${\alpha }_f$ and ${\alpha }_r$ are slip angles at front and rear wheels, respectively.

For the longitudinal motion, commercial speed controllers such as ACC (Adaptive Cruise Controller) are assumed in this study and the longitudinal speed control system is approximated by the first-order delay model.

$$\tau \stackrel{··}{x}+\stackrel{·}{x}=V_{x,\mathrm{targ}}$$

(12)

where $\tau$ is the time constant and $V_{x,\mathrm{targ}}$ is the target speed of the longitudinal speed control system.

In order to represent the dynamic characteristics described in the differential equations (10)–(12) into the global coordinate $(X,Y)$ fixed on the initial position of the host vehicle, the coordinate transformation is needed.

$$\stackrel{·}{X}=\cos \psi \stackrel{·}{x}-\sin \psi \stackrel{·}{y}$$

$$\stackrel{·}{Y}=\sin \psi \stackrel{·}{x}+\cos \psi \stackrel{·}{y}$$

By defining the state variables as follows:

$$\xi ={\left[\begin{array}{c}\begin{array}{cccccc}X\hfill & Y\hfill & \psi \hfill & \stackrel{·}{x}\hfill & \stackrel{·}{y}\hfill & \stackrel{·}{\psi }\hfill \end{array}\hfill \end{array}\right]}^T$$

(13)

Equations (10)–(13) can be expressed into a non-linear state equation with steering angle and target speed as input.

$$\stackrel{·}{\xi }=\left[\begin{array}{c}\cos \psi \stackrel{·}{x}-\sin \psi \stackrel{·}{y}\hfill \\ \multicolumn{1}{c}{\sin \psi \stackrel{·}{x}+\cos \psi \stackrel{·}{y}}\\ \multicolumn{1}{c}{\stackrel{·}{\psi }}\\ \multicolumn{1}{c}{-\stackrel{·}{x}/\tau +V_{x,\mathrm{targ}}/\tau }\\ \multicolumn{1}{c}{-\frac{C_f+C_r}{m}\stackrel{·}{y}/\stackrel{·}{x}-\frac{l_f{C}_f-l_r{C}_r}{m}\stackrel{·}{\psi }/\stackrel{·}{x}-\stackrel{·}{x}\stackrel{·}{\psi }+\frac{C_f}{m}{\delta }_f}\\ \multicolumn{1}{c}{-\frac{l_f{C}_f-l_r{C}_r}{I_z}\stackrel{·}{y}/\stackrel{·}{x}-\frac{{l_f}^2{C}_f+{l_r}^2{C}_r}{I_z}\stackrel{·}{\psi }/\stackrel{·}{x}+\frac{l_f{C}_f}{I_z}{\delta }_f}\end{array}\right]$$

(14)

$$\zeta ={\left[\begin{array}{cc}X\hfill & Y\hfill \end{array}\right]}^T$$

(15)

where $\zeta$ denotes the output vector. The state vector (13) is discretized for MPC formulation.

$${\xi }_{k+1}={\xi }_k+T{\stackrel{·}{\xi }}_k$$

(16)

where $T$ denotes the sampling time of the controller.

Non-convex MPC formulation and modificationof $\mu$ -GA for real-time performance

In this section, a non-convex optimization-based MPC algorithm is designed with a prediction horizon. Firstly, multiple control objectives are defined using a probabilistic function such as lane centering, desired speed tracking, and collision avoidance. Secondly, the formulation for collision avoidance task is proposed considering the real-time performance.

The optimization problem is formulated similar to the conventional MPC, ⁴⁰ but is converted to the gaussian function formulation.

$$\begin{array}{c}{\eta }^{*}=\arg max\sum _{i=1}^n\frac{1}{\sqrt{2\pi {\left|Q\right|}^{-1}}}\exp (-\frac{1}{2}{{\eta }_i}^{T}Q{\eta }_i)\hfill \end{array}$$

(17)

$\begin{array}{c}\mathrm{s}.\mathrm{t}.A\eta \le b\hfill \end{array}$

where $\eta$ is vector of the decision variables and $Q$ is the weight matrix which is inverse of the co-variance matrix. $A$ and $b$ are linear matrix representation of the inequality constraints.

In normal driving case, following the lane center and the desired speed are very important. For the lane centering, the tracking reward of the host vehicle position is described with respect to the center of its own lane:

$$\begin{array}{c}P(Y_{\mathrm{lane},i}|X_k,Y_k)=\mathbb{N}(Y_{\mathrm{lane},i},Q_{\mathrm{lane}}^{-1})\hfill \end{array}$$

(18)

where $\mathbb{N}$ represents the probability density function of the normal distribution and $Q_{\mathrm{lane}}$ is the inverse of the variance of the lateral position of the vehicle. $Y_k$ denotes the predicted lateral position of the host vehicle at the $k^{\mathrm{th}}$ step of the prediction horizon and $Y_{\mathrm{lane},i}$ is the lateral position of the $i^{\mathrm{th}}$ lane.

For multi-lanes, the positional probability can be expressed as follows:

$$P_{\mathrm{lanes}}\left(k\right)=\sum _{i=1}^{n_{\mathrm{lane}}}P(Y_{\mathrm{lane},i}|X_k,Y_k)$$

(19)

where $n_{\mathrm{lane}}$ is the number of lanes. The above equation represents a reward function for tracking lanes and structurally identical to the attractive potential in equation (3).

For the following of the desired speed, its probability is also considered as follows.

$$\begin{array}{c}P(V_{x,\mathrm{des}}|{\stackrel{·}{x}}_k)~\mathbb{N}(V_{x,\mathrm{des}},Q_{V_x}^{-1})\hfill \end{array}$$

(20)

By combining the lane following probability (19) and the desired speed following probability (20), MPC object function for normal driving is formulated.

$$\begin{array}{c}{P}_{\mathrm{ref}}\left(k\right)=P_{\mathrm{lanes}}\left(k\right)P_{V_x}\left(k\right)\hfill \end{array}$$

(21)

For normal driving case, the target steering angle and speed are determined for the prediction horizon, $N_p$ .

$${\left[\begin{array}{cc}{{\delta }_f}^{*}\hfill & {V_{x,\mathrm{targ}}}^{*}\hfill \end{array}\right]}^T=\arg \underset{{\delta }_f,V_{x,\mathrm{targ}}}{min}\sum _{k=1}^{N_p}-P_{\mathrm{ref}}\left(k\right)$$

(22)

where the superscript * represents the optimal or sub-optimal value. This formulation allows the vehicle to change the lane as it travels with following the target speed.

For Collision Avoidance (CA), the control system should consider not only the collision risk, but also the lane keeping and driving at the desired speed. If various driving situations are considered, joint probability for every situation needs to be included and its computation demand can be very extensive. Instead, the following cost function is proposed in this study with less computational load.

$$\mathrm{Cost}\left(k\right)=W_R{R}_{\mathrm{road}}\left(k\right)+W_{\mathrm{col}}{R}_{\mathrm{col}}\left(k\right)-W_{\mathrm{ref}}{P}_{\mathrm{ref}}\left(k\right)$$

(23)

$R_{\mathrm{road}}$ and $R_{\mathrm{col}}$ are described in equations (1) and (7). $W_R$ , $W_{\mathrm{col}}$ , and $W_{\mathrm{ref}}$ are weights for the collision risk, road boundary risk, and reference tracking probability, respectively.

Thus, for collision avoidance case, the target steering angle and the speed are determined as follows for the prediction horizon. Because the CA is much more important than the reference tracking, $R_{\mathrm{col}}$ is set to 1 at the subsequent steps if the collision is detected (see Figure 7).

$${\left[\begin{array}{cc}{{\delta }_f}^{*}\hfill & {V_{x,\mathrm{targ}}}^{*}\hfill \end{array}\right]}^T=\arg \underset{{\delta }_f,V_{x,\mathrm{targ}}}{min}\sum _{k=1}^{N_p}\mathrm{Cost}\left(k\right)$$

(24)

Figure 7.

Collision risk rule for vehicle crashes.

The prediction horizon can be varied depending on the speed. The maximum $N_p$ is set to 30 considering the real-time performance. This selection means that the searching distance at vehicle speed of 16.7 m/s is 25 m with the 50 ms time step. The searching distance becomes smaller than 25 m when the speed is smaller than 16.7 m/s and is greater than 25 m when the speed is greater than 16.7 m/s.

The non-convex optimization problems of equations (22) and (24) are solved by utilizing the $\mu$ -GA method ²⁶ and its configuration is shown in Figure 8. In order to generate the genetic pool for the selection of control input candidates, we need to define the upper and lower bounds of steering angle, ${\delta }_f$ , and target speed, $V_{x,\mathrm{targ}}$ . The range of the steering angle is selected by considering the stability of the vehicle, that is, the limit of the slip angle at the front wheel.

$$\left|{\alpha }_f\right|\le {\left|{\alpha }_f\right|}_{max}$$

where $|{\alpha }_f{|}_{max}$ represent the maximum slip angle allowable in the stable lateral motion. Then, the range of the steering angle can be described using the definition of the lateral tire slip. ³⁹

$${\delta }_{f,min}\le {\delta }_f\le {\delta }_{f,max}$$

(25)

where $\begin{array}{c}{\delta }_{f,min}=-|{\alpha }_f{|}_{max}+\frac{\stackrel{·}{y}+l_f\stackrel{·}{\psi }}{\stackrel{·}{x}}\hfill \\ \multicolumn{1}{c}{{\delta }_{f,max}=|{\alpha }_f{|}_{max}+\frac{\stackrel{·}{y}+l_f\stackrel{·}{\psi }}{\stackrel{·}{x}}}\end{array}$

${\delta }_{f,\min }$ and ${\delta }_{f,\max }$ denote lower bound and upper bound of the steering angle respectively.

Figure 8.

Configuration of $\mu$ -GA.

Similarly, the limit of the target speed ( $V_{x,\mathrm{targ},\min }$ , $V_{x,\mathrm{targ},\max }$ ) is set considering the maximum longitudinal acceleration, $a_{x,\max }$ :

$$V_{x,\mathrm{targ},min}\le V_{x,\mathrm{targ}}\le V_{x,\mathrm{targ},\max }$$

(26)

where $\begin{array}{c}{V}_{x,\mathrm{targ},\min }=V_{x,\mathrm{prior}}-a_{x,max}\tau \hfill \\ \multicolumn{1}{c}{V_{x,\mathrm{targ},\max }=V_{x,\mathrm{prior}}+a_{x,max}\tau }\end{array}$

$V_{x,\mathrm{prior}}$ denotes the target speed at the previous control time step.

The initial population ( $\mathrm{PO}{P}_{\mathrm{ini}}$ ) consists of four parts in the search space defined by the steering angle limit (25) and the target speed limit (26) and selects zero points as an initial fittest. From the divided search space, the experiment points are selected as follows to search evenly while reducing computational complexity. In this study, the initial population is selected uniformly on the control input 4 × 4 grid space without duplication and the Latin hypercube sampling method ⁴¹ is utilized to obtain a more even population than random sampling and to reduce computation time.

• Step 1: Divide the search space of the target speed and the steering angle into three parts separately, and create a 3 × 3 grid map.

• Step 2: Number the index of each point of the grid map sequentially ( $i=1,2,3,4$ and $j=1,2,3,4$ )

• Step 3: Select the index of sample point as $i=1,2,3,4$ and $j=\mathrm{randperm}(4)$

where randperm is an operator that computes the permutation of natural numbers from 1 to 4. The $i^{\mathrm{th}}$ individual of the experiment points selected above is defined as follows:

$$\begin{array}{c}\mathrm{PO}{P}_{\mathrm{ini}}(i)=\left[\begin{array}{cc}{\delta }_{f,min}\hfill & V_{x,\mathrm{targ},min}\hfill \end{array}\right]+\hfill \\ \left[\frac{i}{3}\left(\begin{array}{cc}{\delta }_{f,max}\hfill & {\delta }_{f,min}\hfill \end{array}\right)\right]\frac{j(i)}{3}\left(\begin{array}{cc}{V}_{x,\mathrm{targ},max}-\hfill & V_{x,\mathrm{targ},min}\hfill \end{array}\right)\hfill \end{array}$$

(27)

This approach selects the initial population efficiently and reduces the iterations of the solver. For each population, the tracking probability (21) or the cost function (23) are calculated along the trajectory of the vehicle and the fittest individual is defined with the smallest cost among the population. In this case, the steering angle of an individual is increased or decreased step by step as the horizon proceeds by considering the slew rate of the steering angle.

$${\delta }_f(k+1)=\{\begin{array}{cc}{\delta }_{f,\mathrm{prior}}+{\stackrel{·}{\delta }}_{f,max}\mathrm{kT},\hfill & \mathrm{if}|{\delta }_f(k)|\le {\delta }_{f,\mathrm{pop}}\hfill \\ \multicolumn{1}{c}{{\delta }_{f,\mathrm{pop}}}& \mathrm{else}\hfill \end{array}$$

(28)

where $k$ denotes the prediction point at $\mathrm{kT}$ seconds ahead and ${\stackrel{·}{\delta }}_{f,max}$ is the maximum slew rate of the steering angle. ${\delta }_{f,\mathrm{prior}}$ is the desired steering angle at every control time step. ${\delta }_{f,\mathrm{pop}}$ denotes the steering value of the selected child from a population.

Next, two pairs of parents are chosen in the population through the tournament selection and their overlap is allowed. A pair of parent generates two children using the single-point crossover and a new population of five entities is formed including four children and the fittest individual. In this study, five populations are selected best by considering the trade-off between computational volume and optimization quality. After it converges sufficiently to the local minimum through seven iterations, four entities are newly selected using the equation (27) with the fittest individual. This process is repeated until the convergence criterion is met and the fittest individual is determined as the optimal control input for ${\delta }_f$ and $V_{x,\mathrm{targ}}$ .

Results

To verify the performance of the proposed PMC (Planar Motion Control) system, a number of simulations have been conducted on several scenarios. A cooperative simulation tool has been constructed using software-in-the-loop system: rapid control prototyping unit and high-fidelity vehicle dynamics software. The vehicle model and PMC system parameters are listed in table in the Appendix. The road geometry parameters and traffic information are acquired at every 50 ms. The dynamic states of the host vehicle are transmitted through CAN bus at every 10 ms. The control time step is set to 50 ms.

In the first scenario, the host vehicle accelerates from 0 to 20 m/s on a straight roadway with two lanes. A surrounding vehicle is traveling at 27.8 m/s from 150 m behind. This scenario is one of the common rear-end collision situation on the highway when the driver of the rear vehicle does not pay attention or fall asleep. The simulation results are shown in Figure 9. Trajectories of the vehicles are indicated by dotted line and markers are numbered to represent the simultaneous position of the host vehicle and the vehicle rushing from rear. Also, vehicle edge is described as a rectangular. $V_x$ is the measured speed of the ego-vehicle and $V_{x,\mathrm{des}}$ is the desired speed from the supervisor. $V_{x,\mathrm{targ}}$ is the target speed from the MPC controller and becomes the command to the ACC controller of the vehicle. Rear-end collision is expected when inter-vehicle space is reduced to about 50 m, which is the maximum relative distance to detect collision with the MPC prediction horizon. Because there exist limits for longitudinal acceleration, it has not enough time for the host vehicle to avoid the collision by accelerating. Therefore, the host vehicle avoids the rear-end collision by the automated lane change. The collision avoidance performance is also verified by the cost value result.

Figure 9.

Scenario 1. (a) Trajectories of the host vehicle (red) and obstacle vehicle (blue) coming from behind (b) Steering angle (c) $V_x$ : real speed, $V_{x,\mathrm{des}}$ : desired speed, $V_{x,\mathrm{targ}}$ : target speed.

In the second scenario, two cases of the cut-in are considered. The scenario 2-1 is a situation where CA (Collision Avoidance) is possible by steering alone, and the scenario 2-2 is a case where not only steering, but also deceleration should be engaged. Also, to verify the robustness against the local minima problem, the second obstacle lies with the offset of 0.5 m to the left side of the center line of the lane. In both scenarios, the host vehicle accelerates from 0 to 20 m/s on a straight roadway with two lanes. In scenario 2-1, the obstacle vehicle 1, 30 m ahead in the adjacent lane, is accelerated from 0 to 10 m/s and performs cut-in into the ego lane. Then, the host vehicle confronts a stationary vehicle (obstacle vehicle 2) at 150 m ahead. Simulation results of scenario 2-1 are shown in Figure 10. Since a distance to avoid the vehicle 1 by steering is sufficient, the cost decreases fast during CA maneuver. The proposed controller demonstrates that the host vehicle avoids the collision by a lane change to the left lane instead of steering to the narrow space on the right side of the vehicle 2. In scenario 2-2, all conditions are same except that the obstacle vehicle 1 starts accelerating from 15 m ahead of the host vehicle and the results of Scenario 2-2 are shown in Figure 11. At 7 s, the obstacle vehicle 1 suddenly changes the lane and rushes to the host vehicle, so that the collision can not be avoided by the steering alone. Therefore, the host vehicle is steered and decelerated to avoid collision with vehicle 1. After that, the vehicle starts to accelerate again and reaches the target speed. Then, the host vehicle is steered to the left lane and avoid clash with the obstacle vehicle 2.

Figure 10.

Scenario 2-1. (a) Trajectories of the host vehicle (red), obstacle vehicle 1 (blue) and 2 (green) (b) Steering angle (c) $V_x$ : real speed, $V_{x,\mathrm{des}}$ : desired speed, $V_{x,\mathrm{targ}}$ : target speed.

Figure 11.

Scenario 2-2. (a) Trajectories of the host vehicle (red), obstacle vehicle 1 (blue) and 2 (green) (b) Steering angle (c) $V_x$ : real speed, $V_{x,\mathrm{des}}$ : desired speed, $V_{x,\mathrm{targ}}$ : target speed.

In the third scenario, the host vehicle accelerates from 0 to 20 m/s on a curved roadway with two lanes. The obstacle vehicle is located at 30 m ahead of the adjacent lane and performs cut-in motion with 10 m/s speed. The control performance is illustrated in Figure 12. The host vehicle avoids collision with the cut-in vehicle by changing lane to the right and the host vehicle accelerates to the desired speed. Because there is a stationary vehicle at 150 m ahead, the host vehicle changes lane back to the left lane. More over, the host vehicle slows down to follow the curved road for lateral stability. In the curved roadway, unlike straight one, since the PMC system determines steering effort considering the road geometry, the host vehicle avoids collision with less steering effort in some road sections.

Figure 12.

Scenario 3. (a) Trajectories of the host vehicle (red), obstacle vehicle 1 (blue) and 2 (green) (b) Steering angle (c) $V_x$ : real speed, $V_{x,\mathrm{des}}$ : desired speed, $V_{x,\mathrm{targ}}$ : target speed.

In the fourth scenario, the host vehicle drives through the handling course with various curvature as illustrated in Figure 13. The initial position and speed of the vehicles are also represented in Figure 13. During driving along the road, the host vehicle confronts multiple obstacle vehicles and need to follow the curved road with no collision. The obstacle vehicles travel with slow speed or are stopped on the roadway. The host vehicle should overtake the slow vehicles if necessary and avoid the stopped ones. This scenario mainly considers overtaking slow vehicles on the local road with light traffic. Since there are some winding road sections, the target speed as well as the desired speed are adjusted for the host vehicle to decelerate properly and to maintain its position inside the roadway as shown in Figure 14. Also, the host vehicle changes lanes with an effective steering maneuver to overtake the slow vehicle, to avoid collision and to follow the curved road geometry.

Figure 13.

The location and speed of the vehicle in scenario 4: Indexing the vehicles on the test road (initial X position, initial Y position, longitudinal speed).

Figure 14.

Scenario 4 results. (a): Trajectories of the vehicles in scenario 4 at 25–40 s: the host vehicle (red square), obstacle vehicle 4 (blue triangle), 5 (green pentagram), 6 (magenta inverted triangle), and 7 (indigo circle). (b): Trajectories of the vehicles in scenario 4 at 50–65 s: the host vehicle(red square), obstacle vehicle 8 (blue triangle), and 9 (green pentagram). (c): Steering angle (d) $V_x$ : Real speed, $V_{x,\mathrm{des}}$ : desired speed, $V_{x,\mathrm{targ}}$ : target speed.

Conclusion

In this paper, an autonomous vehicle motion control system is proposed of non-convex optimization based model predictive control (MPC) system for collision avoidance and lane following. In the MPC design, a nonlinear model of vehicle maneuvering is used for optimization without the gradient. For this, the MPC is designed to allow the lane change freely through the probabilistic formulation of multi-reference tracking. The risk penalty of collision is directly applied to the cost function such that the host vehicle can avoid the surrounding vehicles by steering and acceleration.

The performance of the proposed system is evaluated in a vehicle communication environment with the software-in-the-loop system. Computation time of one control step is measured around 15 ms in average, but sometimes 30 ms in the worst case. The presented PMC system is capable of both lateral and longitudinal control, and, in particular, the longitudinal control is actively engaged for collision avoidance and overtaking. The simulation results demonstrate that the proposed system avoids collision by braking, accelerating, or steering for various curved roads with obstacle vehicles.