Receding Horizon Control for Lateral Collision Avoidance of Intelligent Vehicles

Abstract

This paper presents a method of vehicle lateral collision avoidance based on receding horizon strategy. In this paper, the vehicles’ position and velocity information is obtained based on the millimeter-wave radar, the global satellite positioning system (GPS) and the vehicle to vehicle communication technology. Then the method of transforming the vehicles’ position and speed to the host vehicle coordinate system is given. The trajectory prediction model and collision recognition algorithm of the host vehicle and the other vehicle are established based on the kinematic model. The receding horizon control algorithm is used to control the host vehicle speed in order to avoid the coming collision. The simulation results show that the host vehicle can achieve the goal of lateral collision avoidance by receding horizon control algorithm under the condition of constant and changed vehicle speed.

1 Introduction

The increase of the car ownership makes the traffic safety problem even more prominent. The lateral collision of the urban traffic accidents account for up to 27%, which is one of the main types of accidents [1]. Therefore, in recent years, the vehicle lateral collision avoidance problem has been paid more attention and many people have done a lot of researches. In the literature [2], a predictive control method based on the robust model for host vehicle is given in the absence of the control of the other vehicles and without communication between the vehicles. In the literature [3], the changes of the vehicle speed are extracted from the Event Data Recorder (EDR) of the accident vehicle, and the vehicle collision model is constructed and introduced into the I-ADAS to enable lateral collision warning or automatic avoidance function. References [4, 5] propose the vehicle collision avoidance system based on the vehicle and the infrastructure integration. The literature [6, 7] based on DSRC do the research for collision avoidance algorithm.

This paper presents an algorithm using the receding horizon control strategy to achieve the lateral collision avoidance. The structure of this paper is: in the second section, the paper proposes the lateral collision prediction algorithm obtained by the coordinate transformation and the trajectory prediction. In the third section, the paper introduces the receding horizon control algorithm to avoid the coming collision. In the fourth section, we gives the simulation results of this method.

2 Predictive Algorithm for Lateral Collision of Vehicles

At present, the vehicles’ lateral collision avoidance control strategy can be attributed to three categories. The first is to prevent collisions by emergency braking when it is about to crash [6, 8]. The second is to predict the possibility of collision in the future position, by way of early deceleration to avoid it [2, 4]. Unlike the second category, the third category avoids the collision through deceleration and steering [7, 9]. The first strategy is relatively simple to implement for the driver of the mandatory intervention conditions. The second strategy can take into account the driving safety and driving comfort. The driving operation of the second strategy is similar to the operation of the drivers, but it is necessary to predict the trajectory of the other vehicle. The third can be adapted to a more complex driving environment, but it is difficult to achieve. The second type take into account the feasibility and control performance, so we choose the second strategy. To predict the other vehicles’ trajectory, we assume that the host vehicle loads the millimeter-wave radar and GPS, the other vehicles load GPS. And the other vehicles transmit its location and driving direction to the host vehicle through V2V.

The location of the host vehicle and the other vehicles are shown in Fig. 1.

Fig. 1

The coordinate location of the two vehicles

The centroids of the host vehicle and the other vehicle are defined as the origins of the coordinate. The x-axis is the driving direction of the vehicle, the y-axis is perpendicular to the x-axis and it points to the drivers’ side. Then, the host vehicle and the other vehicle coordinate systems are established. The relative distance S between the two vehicles and the angle α between the other vehicle and the x-axis of the host vehicle are measured by the host vehicle’s radar. The translational relationship between the two vehicle coordinate systems is obtained. The origin of the other vehicle relative to the host vehicle coordinate system is (x₂, y₂).

$$ \left\{ {\begin{array}{*{20}l} {x_{2} = S\,\cos \alpha } \hfill \\ {y_{2} = - S\,\sin \alpha } \hfill \\ \end{array} } \right. $$

(1)

The coordinate relationship between the two vehicles is determined by the GPS positioning direction. The x-axis direction of the host vehicle and the other vehicle are $\varphi_{1} ,\varphi_{2}$ got by their loaded GPS [10]. The angle between their x-axis is $\theta { = }\left| {\varphi_{1} { - }\varphi_{2} } \right|$. We can get the angle of the host vehicle relative to the x-axis of the other vehicle.

$$ \beta = \pi - \alpha - \theta = \pi - \alpha - \left| {\varphi_{1} - \varphi_{2} } \right| $$

(2)

Since the other vehicle transmits the longitudinal speed of it through V2V, it is necessary to convert the speed of the other vehicle to the host vehicle coordinate system in order to predict its trajectory relative to the host vehicle. We assume that $v_{x2}$ is the longitudinal velocity of the other vehicle in its own coordinate system and $v_{x2}^{{\prime }} ,v_{y2}^{{\prime }}$ are the speed components of $v_{x2}$ along the x-axis and the y-axis.

$$ \left[ \begin{aligned} v_{x2}^{{\prime }} \hfill \\ v_{y2}^{{\prime }} \hfill \\ \end{aligned} \right] = \left[ {\begin{array}{*{20}c} {\cos \theta } & { - \sin \theta } \\ {\sin \theta } & {\cos \theta } \\ \end{array} } \right]\left[ \begin{aligned} v_{x2} \hfill \\ 0 \hfill \\ \end{aligned} \right] $$

(3)

The other vehicle speed is converted into the speed in the host vehicle coordinate through (3) and we assume it’s unchanged in the prediction horizon. Then we can get the other vehicle’s motion equation in the host vehicle coordinate system.

$$ \left\{ \begin{aligned} x_{2} (k + 1) = x_{2} (k) + v_{{_{x2} }}^{'} (k)T \hfill \\ y_{2} (k + 1) = y_{2} (k) + v_{{_{y2} }}^{'} (k)T \hfill \\ \end{aligned} \right. $$

(4)

The host vehicle’s motion equation is as follow.

$$ \left\{ {\begin{array}{*{20}l} {x_{1} (k + 1) = x_{1} (k) + v_{x1} (k)T} \hfill \\ {y_{1} (k + 1) = y_{1} (k)} \hfill \\ \end{array} } \right. $$

(5)

where T is the sampling period, $v_{x1}$ is the speed of the host vehicle. As the speed of the other vehicle at k + 1 is relevant to $\alpha (k + 1)$ and $\beta (k + 1)$, we can calculate these two angles at any time from Fig. 1.

$$ \begin{aligned} & \alpha (k + 1) = arctan\frac{{\left| {y_{2} (k) - y_{1} (k)} \right|}}{{\left| {x_{2} (k) - x_{1} (k)} \right|}} \\ & \beta (k + 1) = arctan\frac{{\left| {x_{2} (k) - x_{1} (k)} \right|}}{{\left| {y_{2} (k) - y_{1} (k)} \right|}} + \frac{\pi }{2}{ - }\left| {\varphi_{1} - \varphi_{2} } \right| \\ \end{aligned} $$

(6)

According to (4)–(6), the distance at $0 \le i \le T_{p}$ ($T_{p}$ is the prediction horizon) between the center of mass of the two vehicles is:

$$ \Delta S(k + i) = \sqrt {[(x_{1} (k + i) - x_{2} (k + i)]^{2} + [(y_{1} (k + i) - y_{2} (k + i)]^{2} } $$

(7)

We compare the distance between the vehicles $\Delta S(k + i)$ with the safe distance $S_{def}$. If $\Delta S(k + i) \ge S_{def}$ ($0 \le i \le T_{p}$), there is no collision in the prediction horizon, or the collision will occur. If these two vehicles do not collide in the prediction horizon, there is no need to control the speed of the host vehicle. If two vehicles will collide in the prediction horizon, the longitudinal speed of the host vehicle need to be adjusted to avoid the coming collision.

3 Receding Horizon Control Algorithm

If two vehicles are predicted to have a collision according to the method given in Sect. 2, the host vehicle speed should be adjusted for collision avoidance. This section discusses the purpose of adjusting the longitudinal speed of the host vehicle based on the receding horizon control strategy to realize the collision avoidance. Since there is a speed item of the host vehicle to be adjusted in (5), it is necessary to predict the speed of the host vehicle. In this regard, considering the following longitudinal dynamic equation of the host vehicle.

$$ m\dot{v}_{x} = \frac{T}{R} - F_{aero} - F_{r} - F_{s} $$

(8)

where, m is the mass of the host vehicle, T is the total torque of the host vehicle, R is the wheel radius, v_x is the longitudinal velocity of the center of mass. $F_{aero}$ is the host vehicle’s air resistance and $F_{r}$ is the host vehicle’s rolling resistance. We can get:

$$ \dot{v}_{x} = \frac{T}{mR} - \frac{{\rho C_{d} A_{F} v_{x}^{2} }}{2m} - [f_{R0} + f_{R1} \frac{{v_{x} }}{100} + f_{R2} (\frac{{v_{x} }}{100})^{4} ]g $$

(9)

The sampling interval is ΔT = 0.05 s. The torque T is the control quantity and the longitudinal velocity of the host vehicle v_x is the state quantity. Then, we have: u = T, v = v_x. We Taylor expand formula (9) at point v_x (k) and collate to get:

$$ \begin{aligned} v(k + 1) & = \{ 1 - [(\frac{{\rho C_{d} A_{F} x^{*} }}{m} + \frac{{f_{R1} g}}{100} + \frac{{4f_{R2} g(x^{*} )^{3} }}{{100^{4} }})\Delta T]\} v(k) + \frac{\Delta Tu(k)}{mR} \\ & \quad + (\frac{{\rho C_{d} A_{F} (x^{*} )^{2} }}{2m} + \frac{{3f_{R2} g(x^{*} )^{4} }}{{100^{4} }} - f_{R0} g)\Delta T \\ \end{aligned} $$

(10)

We define: $\Delta v(k) = v(k) - v(k - 1)$, $\Delta u(k) = u(k) - u(k - 1)$. Then (10) can be written in the form of the following incremental model.

$$ \Delta v(k + 1) = A\Delta v(k) + B\Delta u(k) $$

(11)

where, $A = 1 - [(\frac{{\rho C_{d} A_{F} x^{*} }}{m} + \frac{{f_{R1} g}}{100} + \frac{{4f_{R2} g(x^{*} )^{3} }}{{100^{4} }})\Delta T]$, $B = \frac{\Delta T}{mR}$.

Based on formula (11), the host vehicle speed prediction model can be obtained.

$$ \Delta V(k + 1\left| k \right.) = S_{v} (k)\Delta v(k) + S_{u} (k)\Delta U(k) $$

(12)

where,

$$ \begin{aligned} & \Delta V(k + 1\left| k \right.) = [\begin{array}{*{20}c} {\Delta v(k + 1\left| k \right.)} & {\Delta v(k + 2\left| k \right.)} & \cdots & {\Delta v(k + p\left| k \right.)} \\ \end{array} ]^{T} \\ & \Delta U(k) = [\begin{array}{*{20}c} {\Delta u(k)} & {\Delta u(k + 1)} & \cdots & {\Delta u(k + p - 1)} \\ \end{array} ]^{T} \\ & S_{v} (k) = \left[ {\begin{array}{*{20}c} A \\ {A^{2} } \\ \vdots \\ {A^{p} } \\ \end{array} } \right],S_{u} (k) = \left[ {\begin{array}{*{20}c} B & 0 & \cdots & 0 \\ {AB} & B & \cdots & 0 \\ \vdots & \vdots & \vdots & \vdots \\ {A^{p - 1} B} & {A^{p - 2} B} & \cdots & B \\ \end{array} } \right] \\ \end{aligned} $$

Figure 2 is the illustration of the collision avoidance between two vehicles. In the figure, A is the position of the collision between two vehicles at k + p when the host vehicle is traveling at the original speed. The point B is the expected position of the host vehicle collision with the other vehicle at time k + p by the use of the control method in this paper.

Fig. 2

The illustration of the collision avoidance between the two vehicles

The safety distance of collision avoidance between two vehicles at k + p (i.e., the distance between A and B) is s*(k + p). According to (12), by adjusting the speed of the host vehicle to avoid the collision can be described as the following problem.

The optimization problem 1:

$$ \begin{aligned} & Find\begin{array}{*{20}c} {} & {} \\ \end{array} \mathop {\hbox{min} }\limits_{\Delta u(k),\Delta u(k + 1) \cdots \Delta u(k + p - 1)} \begin{array}{*{20}c} {} & J \\ \end{array} \\ & with\begin{array}{*{20}c} {} & {} \\ \end{array} J = \sum\limits_{i = 0}^{p} {\left\| {\Delta v(k + i)} \right\|_{Q}^{2} } + \sum\limits_{i = 0}^{p} {\left\| {\Delta u(k + i)} \right\|_{R}^{2} } \\ & \begin{array}{*{20}c} {} & {} & {} & {} \\ \end{array} \quad + L_{p} \left| {\sum\limits_{i = 0}^{p} {\Delta v(k + i)\Delta T} - s^{*} (k + p)} \right| \\ \end{aligned} $$

(13)

s.t.

$$ \Delta V(k + 1\left| k \right.) = S_{v} (k)\Delta v(k) + S_{u} (k)\Delta U(k) $$

(14)

$$ \Delta u_{\hbox{min} } (k + i) \le \Delta u(k + i) \le \Delta u_{\hbox{max} } (k + i) $$

(15)

$$ u_{\hbox{min} } (k + i) \le u(k + i) \le u_{\hbox{max} } (k + i) $$

(16)

The objective function J of the optimization problem 1 consists of three terms: the first term is weighting term of the speed change; the second term is weighting term of the driving torque change, and the third term is weighting term of the error between the host vehicle actual position and desired position at k + p, it is used to constrain the error of the actual position and the desired position. In order to get the optimal solution of problem 1, the problem is written as follow.

$$ \begin{aligned} \mathop {\hbox{min} }\limits_{U} J & = \Delta V(\left. {k + 1} \right|k )^{T} Q\Delta V(\left. {k + 1} \right|k )+ \Delta {\text{U}}^{T} (k)R\Delta U(k) + L_{p} [s^{*} (k + p)]^{2} \\ & \quad + \Delta T_{m} \Delta V(\left. {k + 1} \right|k )L_{p} \Delta V(\left. {k + 1} \right|k )^{T} \Delta T_{m}^{T} - 2[\Delta V(\left. {k + 1} \right|k )^{T} \Delta T_{m}^{T} + \Delta v(k)\Delta T \cdot s^{*} ] \\ \end{aligned} $$

(17)

We can get the next formula from (17).

$$ \mathop {\hbox{min} }\limits_{U} J\mathop \Rightarrow \limits^{\text{Equivalent}} \tilde{J} = \Delta {\text{U}}^{T} (k)H\Delta U(k) + 2F^{T} \Delta U(k) $$

(18)

where, $\left\{ {\begin{array}{*{20}l} {H = R + S_{u}^{T} QS_{u} + S_{u}^{T} \Delta T_{m}^{T} L_{p} \Delta T_{m}^{{}} S_{u} } \hfill \\ {F = - L_{p} s^{*} S_{u}^{T} \Delta T_{m}^{T} } \hfill \\ \end{array} } \right.$, $\Delta T_{m}^{{}} = [\begin{array}{*{20}c} {\Delta T} & \cdots & {\Delta T} \\ \end{array} ]_{{1*T_{p} }}$.

Obviously, the optimization problem described by (18) can obtain the optimal solution by QP algorithm. For the sake of clarity, the vehicle lateral collision avoidance control method is summarized as follows.

• Step 1 We measure the relative position and angle of the other vehicle through the host vehicle’s radar and we get the heading angle of two vehicles through the GPS.

• Step 2 At the moment of k, through the V2V to obtain the other vehicle’s speed and heading angle, we converse the positions, speeds and relative angles to the host vehicle’s coordinate system.

• Step 3 The trajectory prediction model of the vehicles in the host vehicle coordinate is established at k, and we determine whether the two vehicles may crash based on it.

• Step 4 For the case of possible collision, we use the receding horizon control algorithm to control the host vehicle. According to the principle of receding optimization, we get the driving torque sequence at time k by solving the optimization problem 1: $\Delta U(k) = [\begin{array}{*{20}c} {\Delta u(k)} & {\Delta u(k + 1)} & \cdots & {\Delta u(k + p - 1)} \\ \end{array} ]^{T}$. We use $\Delta u(k)$ to control the host vehicle so that the collision between the vehicles can be avoided. Repeat the above process, by optimizing the control of each moment to achieve the collision avoidance between the host vehicle and other vehicles.

4 Simulation Results and Analysis

Host vehicle parameters and simulation parameters used in the simulation experiment are shown in Table 1.

Table 1 Host vehicle and simulation parameters

The internal time of the drawing cars in the following figures are 0.5 s.

Simulation condition 1: the other vehicle speed is constant.

This simulation condition is used to examine the feasibility and effectiveness of the host vehicle collision avoidance algorithm when the vehicle speed is kept constant. The initial speed of the host vehicle is 17 m/s and the initial speed of the other vehicle is 16 m/s. The time of the prediction is 3 s, and the internal time is 0.05 s. Then we can get the length of the prediction horizon $T_{p} { = }60$. It is known that the initial distance between two vehicles is 80 m, and the angle of two vehicles’ heading direction is $\alpha (k) = 45^{ \circ } ,\beta (k) = 47^{ \circ }$ (Fig. 1). According to formula (1), the coordinate of the other vehicle in the coordinate system of the host vehicle can be shown as:

$$ [x_{2} (k),y2(k)]^{'} = [56.57, - 56.57]^{'} $$

The simulation supposes that the safe distance between the two vehicles is S_def = 30 m, and the relative distance to avoid the collision between them is s*(k + p) = 15 m. According to the collision avoidance algorithm in the Sect. 2 of this paper, if the host vehicle is not controlled, the distance between two vehicles is 1.15 m at 3.5 s as shown in Fig. 3a and the other vehicle is in the collision risk area (which is shown in red). Figure 3b shows that the two vehicle trajectories got through the receding horizon control method given in the third section. The figure shows that the adjustment of the longitudinal speed of the host vehicle by the receding horizon control algorithm can make the other vehicle avoid being in the collision area of the host vehicle to realize the collision avoidance.

Fig. 3

a The host vehicle is not in control. b The host vehicle is in control

Figure 4 shows how the host vehicle speed changes with time. From Fig. 4, we can know that the host vehicle speed rapidly reduces to 8 m/s in the initial 3 s, when the other vehicle is in the collision risk area. After 3.5 s, the other vehicle drives away from the collision risk area, and the host vehicle speed begins to gradually return to 17 m/s until about 10 s. Figure 5 shows how the host vehicle acceleration changes with time. In the deceleration process, the maximum acceleration is about −0.6 g. 2.5 s later, the acceleration begins to gradually increase and 3.5 s later, it returns to a positive value. After the other vehicle drives away from the collision risk area, the host vehicle begins to speed up.

Fig. 4

The speed changes with time

Fig. 5

The acceleration changes with time

• Simulation condition 2: the other vehicle speed changes.

The simulation condition 2 is used to test the feasibility and effectiveness of the algorithm in this paper in the case that the vehicle speed changes. It is supposed that S_def and s^* (k + p) are the same as the simulation condition 1. And the initial speed, relative position and other parameters of the host vehicle and the other vehicle are also the same as the condition 1. It is supposed that the other vehicle speed changes as shown in Fig. 6: from 0 to 1.5 s, v_x2 = 16 m/s; from 1.5 to 4.5 s the speed decelerates; at the time of 4.5 s, the speed reduces to 0; from 6 to 9 s, the other vehicle accelerates until the speed increases to 16 m/s at 9 s.

Fig. 6

The speed change of the other vehicle

The collision avoidance algorithm in the Sect. 2 of this paper shows: if the host vehicle is not controlled, the distance between the two vehicles is 11.57 m at 3.5 s and it is in the collision risk area (which is shown in red). Figure 7b shows that the adjustment of the longitudinal speed of the host vehicle by the receding horizon control algorithm can make the other vehicle avoid being in the collision area of the host vehicle to realize the collision avoidance.

Fig. 7

a The host vehicle is not in control. b The host vehicle is in control

Figure 8 shows how the host vehicle speed changes with time. It shows that the host vehicle speed in the initial 4 s quickly reduces to about 2 m/s. 4 s later, the other vehicle drives away from the collision area and the host vehicle speed increases gradually. Figure 9 shows how the host vehicle acceleration changes with time. In the process of host vehicle speed reduction, the maximum acceleration is about −0.8 g. 3.5 s later, the acceleration began to gradually increase. 4 s later, it returns to a positive value. After the other vehicle drives away from the collision area, the host vehicle begins to speed up.

Fig. 8

The speed changes with time

Fig. 9

The acceleration changes with time

5 Conclusion

In this paper, the collision recognition algorithm and the receding horizon optimization algorithm make the other vehicle always be outside the collision risk area of the host vehicle. After judging there is no collision risk of two vehicles, the host vehicle returns to the normal speed, while ensuring the safety of vehicles and their driving performance at the same time. The simulation results show that when the other vehicle is in the collision risk area, the host vehicle speed decreases to avoid the collision between the two vehicles according to the receding horizon strategy. When the other vehicle drive away from the collision risk area, the host vehicle speed increases incrementally and eventually returns to normal.