Trajectory Planning with Time-Variant Safety Margin for Autonomous Vehicle Lane Change ^†

Abstract

A lane change is one of the most important driving scenarios for autonomous driving vehicles. This paper proposes a safe and comfort-oriented algorithm for an autonomous vehicle to perform lane changes on a straight and level road. A simplified Gray Prediction Model is designed to estimate the driving status of surrounding vehicles, and time-variant safety margins are employed during the trajectory planning to ensure a safe maneuver. The algorithm is able to adapt its lane changing strategy based on traffic situation and passenger demands, and features condition-triggered rerouting to handle unexpected traffic situations. The concept of dynamic safety margins with different settings of parameters gives a customizable feature for the autonomous lane changing control. The effect of the algorithm is verified within a self-developed traffic simulation system.

1. Introduction

Lane changing (LC) is among the most frequent scenarios encountered in daily driving, and considered as one of most important research topics for autonomous vehicles and advanced driver assistance systems [1]. Current research on autonomous driving encompasses different fields, including perception, planning, and control. The lane change maneuver of autonomous vehicle is considered as a challenge since the control involves changes in both the longitudinal and lateral velocity as well as the movement of surrounding vehicles [2]. In this paper, a trajectory planning method for autonomous lane change is investigated on the basis of the existing perceptive information of traffic vehicles. In [3], LCs are classified into mandatory and discretionary ones. This paper focuses on the latter, which is intended to improve the driving condition of the controlled vehicle.

Various research studies have focused on the trajectory planning and motion control of autonomous driving technologies [4,5,6]. In order to achieve good results in the respective lane change maneuvers, most of these proposed cooperative planning algorithms for autonomous driving employed a rule-based control [7,8] or an optimization-based control [2,9]. However, to solve the problems of computational complexity, the trajectory planning algorithm often assumes a given reference trajectory or considers either the longitudinal or the lateral aspects of the planning problem. A dynamic collision avoidance constraints is necessary for the autonomous lane change maneuvers. In [10,11] the trajectory planning method of lane change maneuvers was studied, but none of the methods considered the position constraint for the collision avoidance. The papers [12,13] attempt to generate large numbers of trajectory candidates by the state-space sampling method, and choose the best one based on collision detection and kinematic limitations. This method has good effectiveness and robustness, but the computation complexity would be a challenge for the real-time implementation. In [14] a trajectory planning algorithm is designed by the quadratic programming method to achieve kinematic constraint in the hazard avoidance scenarios, but the proposed approach assumes constant longitudinal velocity without the consideration of lateral trajectory planning. In [15] a convex optimization method for the trajectory planning during the collision avoidance was applied. However, it employed a given reference trajectories without continuous path re-planning for real application.

The trajectory planning is based on an interactive model that captures mutual influences among all surrounding cars. The difficulties of the lane change maneuvers are the mutual interactions between the host vehicle and the surrounding cars. If the information of all traffic vehicles can be known, the first issue is the determination of the right place and moment for lane change. The solution offered in [16] makes the decision using a complicated logic tree, specifying an action for every scenario that can be thought of, but some special cases still may not be covered. Another solution proposed in [17,18] attempts to replicate human decision-making behavior using Fuzzy Inference System, but the modulation of fuzzification and defuzzification parameters needs considerable experience. Additionally, they only consider two lanes and a predefined number of vehicles, and output binary results. The second issue is to generate proper reaction and trajectory for the lane change maneuver. In practice, one-off trajectory planning is unsuitable in practice, since the unpredicted changes of the surrounding vehicles make the planned path be no longer optimal or even unsafe. A continuous trajectory planning is applied in [12], but this method requires large amount of calculation. A balance should be found between the algorithm complexity and efficiency.

To overcome these limitations, the lane change maneuver algorithm proposed in this paper considers both the longitudinal and the lateral planning in a dynamic traffic. The LC algorithm is expected to have robustness to deal with the unexpected road events, while not inducing too heavy calculated load. The availability of lane change manoeuvres is increased by reducing the required margins to ensure a safe manoeuvre [19]. With these in mind, an algorithm with condition-triggered safety margin function is proposed for the trajectory re-planning control of lane change. The trajectory planning is designed with cost function to balance the safety, comfort and efficiency according to traffic condition. The time-variant safety margins is designed with the longitudinal and lateral constraints to avoid collision. This method can improve the success rate of lane change, and also reduce the computational complexity. Furthermore, by the prediction of traffic states, an evaluation model to select an appropriate inter-vehicle traffic gap and time instance for lane change is proposed. In the final step, by the comparison with the existing methods in a traffic simulation system, the results verify the effectiveness and robustness of algorithm under various traffic scenarios.

The remainder of this paper is organized as follows. The detailed methodology of the lane change control is described in Section 2, including the algorithm design of gap evaluation, trajectory planning and time-variant safety margin. Simulation analysis with random traffic scenarios and specified traffics are given in Section 3. The conclusions of this paper are presented in Section 4.

2. Methodology

The control algorithms of lane change maneuvers generally contain three consecutive parts: decision-making system for the start of lane change, generating a reference path for the vehicle to follow, and real time path re-planning during the overall process. To take the analysis of algorithm, the space between the two vehicles on the road is defined as a traffic gap. The host vehicle $V_H$ is controlled to perform a lane change maneuver, and it will move from one traffic gap in the current lane to another traffic gap in a neighboring lane. To simplify the algorithm design, the lane change maneuver is taking place in adjacent lanes, without the consideration of other specific maneuvers, such as crossing multiple lanes at once.

Figure 1 shows the flowchart of the lane change algorithm, which features three major parts: evaluation of available traffic gaps, trajectory planning and trajectory correction. The lane change algorithm is carried out in a loop cycle for the real-time control, and one cycle is executed per time step $t_s$. For each cycle, monitored traffic information is updated, and input into the lane change algorithm for calculation.

When the host vehicle $V_H$ is controlled for a lane change maneuver, the algorithm cycle will start as the control diagram in Figure 1. Firstly, by using the monitored and predicted traffic information, the algorithm will evaluate available traffic gap for lane change (Part A). When the target traffic gaps better than the current driving gap are determined, the algorithm attempts to generate LC trajectory for these gaps with relative constraints. The traffic gap with successful trajectory planning and highest evaluation index will be selected for lane change. If none gap is successful, $V_H$ stays in its original gap, otherwise, an optimal trajectory for lane change is determined for $V_H$ to follow (Part B). During the lane changing, the viability of current LC trajectory is constantly monitored, and if it is no longer feasible, a trajectory correction is required. Thus a trajectory re-planning control is conducted to satisfy the changed constraints (Part C). The time cost for one calculation cycle is related to the current stage of lane change maneuver. Based on the hardware environment, three stages shown in Figure 1 have different calculation time. To satisfy the real-time requirement, the time cost for one cycle should be controlled below 50 ms. All procedures above will be explained in the following Sections.

2.1. Gap Evaluation

This subsection covers the methods used in Part A of the algorithm, which are used to evaluate the available gaps for lane change. For each lane, we only consider the single gap in which the current longitudinal position of $V_H$ resides. This means we only need to examine two or three gaps, which is in accordance to the fact that ordinary sensors on autonomous vehicles can only guarantee detection of the nearest vehicles in current and adjacent lanes, as Figure 2 shows. Besides, the solution is simplified and calculated load can be significantly reduced.

2.1.1. Traffic Prediction with Gray Prediction Model

Assuming the total number of vehicles to consider is N, for the ith vehicle $\left(1⩽i⩽N\right)$, we denote its speed series measured at discrete time instances $\left\{1,2,\dots ,m\right\}$ as $\left\{v_i\left(1\right),v_i\left(2\right),\dots ,v_i\left(m\right)\right\}$, where m is the present time instance. To balance the algorithm complexity and efficiency, here a Gray Prediction Model is employed to approach the traffic prediction [20]. The sequence of future vehicle speeds $\left\{v_i(m+1),v_i(m+2),\dots ,v_i\left(n\right)\right\}$ for discrete time instances $\left\{m+1,\dots ,n\right\}$ can be obtained by the following calculations. We denote $\left\{X_i\left(1\right),X_i\left(2\right),\dots ,X_i\left(m\right)\right\}$ as the accumulation of $v_i$:

$$X_i\left(k\right)=\sum _{j=1}^k{v}_i\left(j\right),1⩽k⩽m$$

(1)

Here $X_i$ is a monotonically increasing sequence, which is suitable for the exponential fitting. Assuming $X_i$ in general satisfies the following differential equation:

$$\Delta X_i\left(k\right)+a{X}_i\left(k\right)=u$$

(2)

where a and u are constant scalar parameters, then solving for $X_i$ by an exponential sequence gives

$$X_i(k+1)=[X_i\left(1\right)-\frac{u}{a}]e^{-ak}+\frac{u}{a}$$

(3)

Since $\Delta X_i\left(k\right)=X_i\left(k\right)-X_i(k-1)=v_i\left(k\right)$, and $X_i\left(k\right)$ can be replaced by the approximation $\frac{1}{2}[X_i(k-1)+X_i\left(k\right)]$, the Equation (2) can be rewritten in matrix form as

$$v_i\left(k\right)=\left[\begin{array}{cc}-\frac{1}{2}(X_i(k-1)+X_i\left(k\right))& 1\end{array}\right]\left[\begin{array}{c}a\\ u\end{array}\right]$$

(4)

Let

$$Y_i={\left[\begin{array}{ccc}{v}_i\left(2\right)& \dots & v_i\left(m\right)\end{array}\right]}^T$$

(5)

$$B_i=-\frac{1}{2}\left[\begin{array}{cc}{X}_i\left(1\right)+X_i\left(2\right)& -2\\ X_i\left(2\right)+X_i\left(3\right)& -2\\ ⋮& ⋮\\ X_i(m-1)+X_i\left(m\right)& -2\end{array}\right]$$

(6)

By the least square method, the estimation of a and u is solved as

$${\left(a_i,u_i\right)}^T={\left(B_i^T{B}_i\right)}^{-1}{B}_i^T{Y}_i$$

(7)

The speed prediction for discrete time instances $\left\{m+1,\dots ,n\right\}$ is calculated as

$$v_i\left(k\right)=X_i\left(k\right)-X_i(k-1),m+1⩽k⩽n$$

(8)

where the prediction of $X_i\left(k\right)$ is obtained by the fitting result according to the exponential rule

$$X_i\left(k\right)=\left(X_i\left(1\right)-\frac{u_i}{a_i}\right)e^{-a_i(k-1)}+\frac{u_i}{a_i}$$

(9)

Due to traffic randomness, the accuracy of prediction by Gray Prediction Model can deteriorate quickly as prediction horizon extends. Since typical lane changes have a duration of 3–6 s, during the prediction of each traffic vehicle status, we set the prediction horizon $t_p=4\mathrm{s}$. Based on general road standards and vehicular kinetic characteristics, the velocity and acceleration of host vehicle $V_H$ is subject to the parameter limitations listed in

Table 1. Predefined Constraint Parameters.

Symbol	Value	Symbol	Value	Symbol	Value
$v{x}_{min}$	$15\mathrm{m}/\mathrm{s}$	$a{x}_{min}$	$-2\mathrm{m}/{\mathrm{s}}^2$	$j{x}_{min}$	$-5\mathrm{m}/{\mathrm{s}}^3$
$v{x}_{max}$	$30\mathrm{m}/\mathrm{s}$	$a{x}_{max}$	$2\mathrm{m}/{\mathrm{s}}^2$	$j{x}_{max}$	$5\mathrm{m}/{\mathrm{s}}^3$
$v{y}_{min}$	$-2\mathrm{m}/\mathrm{s}$	$a{y}_{min}$	$-2\mathrm{m}/{\mathrm{s}}^2$	$j{y}_{min}$	$-5\mathrm{m}/{\mathrm{s}}^3$
$v{y}_{max}$	$2\mathrm{m}/\mathrm{s}$	$a{y}_{max}$	$2\mathrm{m}/{\mathrm{s}}^2$	$j{y}_{max}$	$5\mathrm{m}/{\mathrm{s}}^3$
$M_{vx1}$	$15\mathrm{m}/\mathrm{s}$	$M_{ax1}$	$6\mathrm{m}/{\mathrm{s}}^2$	$M_{jx1}$	$15\mathrm{m}/{\mathrm{s}}^3$
$M_{vx2}$	$10\mathrm{m}/\mathrm{s}$	$M_{ax2}$	$2\mathrm{m}/{\mathrm{s}}^2$	$M_{jx2}$	$15\mathrm{m}/{\mathrm{s}}^3$
$M_{vy1}$	$2\mathrm{m}/\mathrm{s}$	$M_{ay1}$	$2\mathrm{m}/{\mathrm{s}}^2$	$M_{jy1}$	$15\mathrm{m}/{\mathrm{s}}^3$
$M_{vy2}$	$2\mathrm{m}/\mathrm{s}$	$M_{ay2}$	$2\mathrm{m}/{\mathrm{s}}^2$	$M_{jy2}$	$15\mathrm{m}/{\mathrm{s}}^3$

. These parameters is set to allow enough maneuverability for regular driving, while retaining an acceptable level of ride comfort.

2.1.2. Gap Rating

To determine which traffic gaps are more favorable, all available gaps are rated with an evaluation function. Based on the analysis of human decisions during a lane change driving [21], the score of each traffic gap is a weighted sum of vehicle dynamic parameters:

$$S_g=w_1^T{d}_{FH}+w_2^T{v}_F+w_3^T{d}_{FR}$$

(10)

where $S_g$ is the evaluated score of a target traffic gap; $d_{FH}$ is the longitudinal distance between the leading vehicle and the host vehicle $V_H$; $v_F$ is the speed of the leading vehicle at the target traffic gap; and $d_{FR}$ is the longitudinal distance between the leading vehicle and the following vehicle, which indicating the size of the gap. It is noted that $d_{FH}$,$v_F$,$d_{FR}$ are discrete series formulated as the column vectors within the prediction horizon. $w_1$,$w_2$,$w_3$ are column vectors of weight coefficients. The parameters of evaluation function are shown in Figure 3.

By adjusting the weight coefficients $w_i$, the evaluation function can be designed based on the desired optimization objective. Because the prediction accuracy will deteriorate when the prediction horizon becomes longer, the weight coefficients are designed as

$$w_i\left(k\right)=w_i(m+1)e^{\beta (k-m-1)},m+1⩽k⩽n$$

(11)

where $\beta$ controls the decaying rate of $w_i$.

2.2. Trajectory Planning with Time Variant Safety Margin

Based on the results of evaluation, traffic gaps with higher scores than the current one will be set as the targets for trajectory planning. It should be noted that the gap with the highest score may not become the final choice, since a lane changing trajectory to that gap may fail to be planned. The order of trajectory planning for each traffic gap is arranged by their evaluated score. Therefore, the traffic gap with relatively better evaluation index and successful trajectory planning can be chosen as the final lane changing target.

2.2.1. Trajectory Planning

During the trajectory planning of vehicle, safety and comfort are two influential factors for the performance evaluation. The safety mainly refers to avoid potential collisions, which keep a distance away from other vehicles; and the comfort refers to minimize the variation of vehicle movement, such as the changing rate of velocity and acceleration. In order to consider these factors during the trajectory planning, a cost function for the optimum programming is designed in this section.

The basic cost function for trajectory planning is designed to minimize the changes of the motion status of host vehicle $V_H$, such as vehicle speed, acceleration and jerk, while keeping them within the longitudinal and lateral constraints. Here the longitudinal and lateral trajectory planning employ similar cost functions by utilizing a Quadratic Programming (QP) method. The function for the longitudinal trajectory planning is defined as

$$\begin{array}{cc}\hfill J_x& =q_{1x}{\left|v{x}_H-v{x}_{H,des}\right|}^2+q_{2x}{\left|a{x}_H\right|}^2\hfill \\ \hfill & +q_{3x}{\left|j{x}_H\right|}^2+q_{ϵx}\left({\left|{ϵ}_{x1}\right|}^2+{\left|{ϵ}_{x2}\right|}^2\right)\hfill \\ \hfill s.t.& \left[\begin{array}{c}v{x}_{min}\\ a{x}_{min}\\ j{x}_{min}\end{array}\right]-{ϵ}_{x1}⩽\left[\begin{array}{c}v{x}_H\\ a{x}_H\\ j{x}_H\end{array}\right]⩽\left[\begin{array}{c}v{x}_{max}\\ a{x}_{max}\\ j{x}_{max}\end{array}\right]+{ϵ}_{x2}\hfill \\ \hfill & \left[x_H\left(1\right),v{x}_H\left(1\right),a{x}_H\left(1\right)\right]=\left[x_{H1},v{x}_{H1},a{x}_{H1}\right]\hfill \\ \hfill & 0⩽{ϵ}_{xi}⩽\left[\begin{array}{c}{M}_{vxi}E\\ M_{axi}E\\ M_{jxi}E\end{array}\right],i=1,2\hfill \end{array}$$

(12)

where the longitudinal position, speed, acceleration and acceleration jerk of $V_H$ are represented as $x_H$, $v{x}_H$, $a{x}_H$ and $j{x}_H$, they are discrete series formulated as the column vectors with length $k_{end}$, and $k_{end}$ is the length of the discretization for prediction horizon. $E={\left[1,1,\dots ,1\right]}^T$ is a column vector also with length $k_{end}$; $v{x}_{H,des}$ is the desired longitudinal speed of $V_H$; $q_{1x}$, $q_{2x}$, $q_{3x}$ and $q_{ϵx}$ are weight coefficients. The initial parameter values $x_{H1}$, $v{x}_{H1}$ and $a{x}_{H1}$ are obtained through measurement at the start of lane change.

To improve the success rate of trajectory planning, slack variables ${ϵ}_{x1}$ and ${ϵ}_{x2}$ are employed to loosen the constraints. The slack variables are only applied during trajectory re-planning due to unexpected traffic events. The upper bounds of the slack variables are determined by $M_{vxi}$, $M_{axi}$ and $M_{jxi}$. The cost function of the lateral trajectory planning can be obtained by change the parameters of Equation (12) with $y_H$, $v{y}_H$, $a{y}_H$, $j{y}_H$, $v{y}_{H,des}$, $q_{1y}$, $q_{2y}$, $q_{3y}$, $q_{ϵy}$, $M_{vyi}$, $M_{ayi}$ and $M_{jyi}$. All of the predefined longitudinal and lateral constraints are listed in Table 1. The velocities of all traffic vehicles will be varied within the range of the constraints.

For the algorithm of trajectory planning, we need to specify a planning horizon. Based on the prediction horizon for the traffic vehicles, it is reasonable to set the trajectory planning horizon the same as $t_p$. Since the actual time horizon for LC process is less than the planning horizon. In order to design the relative algorithm, the finished time horizon for the LC is defined as $t_{fin}$, which can be calculated by

$$t_{fin}=\min \left[t_{gc}-t_1,\frac{\left(t_p-t_2\right)\left|y_{init}-y_{tgt}\right|}{w_l}+t_2\right]$$

(13)

where $t_{gc}$ is the time left before current and target gap no longer intersect; $y_{init}$ is the initial lateral position of $V_H$, $y_{tgt}$ is the lateral coordinate of target lane centerline; $t_1$ and $t_2$ are constant times, $t_1$ is used to make sure LC is finished before gap intersection disappears; $t_2$ is used to leave enough time when $y_{init}$ is close to $y_{tgt}$; and $w_l$ is the width of one traffic lane.

As a pre-requirement for performing numerical computing for our simulation experiment, $t_p$ and $t_{fin}$ are discretized by the algorithm time step $t_s$. We then define $k_{end}=t_p/t_s+1$ as the length of the discretized planning horizon, and $k_{fin}=t_{fin}/t_s+1$ as the length of the time horizon for the finish of lane change.

2.2.2. Supplementary Longitudinal Constraints

During the lane change, the traffic states is in a dynamic and ongoing process. In addition to the predefined constraints of host vehicle in Equation (12), a supplementary dynamic constraints is proposed for the trajectory planning. In the time-varying traffic flow, a safety margin concept with the dynamic constraints is employed to improve the lane changing control effect. In this part, the calculation of the supplementary constraints used in longitudinal trajectory planning is investigated.

The longitudinal position constraint sequence $x_{min}$ and $x_{max}$ are defined for the host vehicle $V_H$. These constraints are sequences made up of the lower and upper boundary of current and target lane safe region intersection at each time instance within the planning horizon. From the Figure 4, the horizontal axis is the discrete time sequence k, $k_{fin}$ is the finished time instance of lane change, and $k_{end}$ is end time instance of planning horizon. The vertical axis represents the longitudinal displacement of the host vehicle $V_H$ during the lane change. The yellow colored region is the safe zone of $V_H$ with respect to time, and its boundaries are determined by $x_{min}$ and $x_{max}$.

Since the safe region intersection of the current and target lane, constraint sequence $x_{min}$ and $x_{max}$ are given by

$$\begin{array}{c}\hfill \begin{array}{c}\hfill x_{min}\left(k\right)=\left\{\begin{array}{c}\hfill \max \left[x_{min}^t\left(k\right),x_{min}^s\left(k\right)\right],k⩽k_{fin}\\ \hfill x_{min}^t\left(k\right),k_{fin}<k⩽k_{end}\end{array}\right.\\ \hfill x_{max}\left(k\right)=\left\{\begin{array}{c}\hfill \min \left[x_{max}^t\left(k\right),x_{max}^s\left(k\right)\right],k⩽k_{fin}\\ \hfill x_{max}^t\left(k\right),k_{fin}<k⩽k_{end}\end{array}\right.\end{array}\end{array}$$

(14)

where $x_{min}^t$ and $x_{max}^t$ are constraints determined by the gap on the target lane, while $x_{min}^s$ and $x_{max}^s$ are their counterparts on the current lane. Calculation method is the same for $x_{min}^t$, $x_{max}^t$ and $x_{min}^s$, $x_{max}^s$, so we use $x_{min}^g$, $x_{max}^g$ to refer to either of them:

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill & x_{min}^g\left(k\right)=x_R\left(k\right)+s_R^t\left(k\right)+s_R^a\left(k\right)\hfill \\ \hfill & x_{max}^g\left(k\right)=x_F\left(k\right)-s_F^t\left(k\right)-s_F^a\left(k\right)\hfill \end{array}\end{array}$$

(15)

where $x_R$ is the head position sequence of following vehicle and $x_F$ is the tail position sequence of leading vehicle. An illustration of the longitudinal constraint sequence during the lane change is shown in Figure 5.

$s_R^t$ and $s_F^t$ are safety margins related to vehicle speed, which are calculated by

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill & s_R^t\left(k\right)=v_R\left(k\right)t_g+d_x+L\hfill \\ \hfill & s_F^t\left(k\right)=min\left[v{x}_{max},v_F\left(k\right)\right]t_g+d_x+L\hfill \end{array}\end{array}$$

(16)

where $v_R$ and $v_F$ are the speeds of following and leading vehicle; $t_g$ is a desired time gap for two vehicles; $d_x$ is the minimal safe distance between $V_H$ and other vehicle; and L is the length of vehicle. Here, setting $t_g$ too small (resulting in small safety margin and loose constraint) may cause frequent trajectory invalidation due to even minor unexpected traffic vehicle movement, while doing the contrary raises the size requirement for valid gaps, thereby wasting potential LC opportunities. We thus introduce additional safety margins $s_R^a$ and $s_F^a$, which start small, then monotonically increases towards the end of the planning horizon, corresponding to the rise of traffic vehicles movement uncertainty with respect to time. For simplicity, we define the new time-variant safety margin as

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill & s_F^a\left(k\right)=\left(k-1\right)K_F\hfill \\ \hfill & s_R^a\left(k\right)=\left(k-1\right)K_R\hfill \end{array}\end{array}$$

(17)

With this simple linear relationship, we can conveniently use parameters $K_F$ and $K_R$ to adjust the size of $s_R^a$ and $s_F^a$. A larger $K_F$ and $K_R$ sets larger safety margins around traffic vehicles, which brings a higher success rate for the lane change, but also more conservative trajectory planning. Conversely, a smaller $K_F$ and $K_R$ can bring more aggressive trajectory planning.

Since the limitation of the vehicle ability to brake in a high velocity, the maximum speed at the end of lane change should be controlled to avoid planning failure. Here we set an upper limit for the longitudinal velocity at the finished time instance $k_{fin}$ as

$$v{x}_H\left(k_{fin}\right)⩽v_F\left(k_{fin}\right)+\sqrt{2\left|a{x}_{min}\right|\Delta x_{fin}}$$

(18)

where

$$\Delta x_{fin}=x_{max}\left(k_{fin}\right)-x_H\left(k_{fin}\right)$$

(19)

With this limitation, the end velocity of $V_H$ can be controlled to satisfy the deceleration capacity, and the planned trajectory can stay out of the safety margin of front vehicles.

2.2.3. Supplementary Lateral Constraints

The lateral constraints for lane change control is derived from the boundaries of lane width. In this part, the calculate of the position constraints used in lateral trajectory planning is investigated. Similar to those used for longitudinal trajectory planning, position constraint sequences $y_{min}$ and $y_{max}$ for lateral movement are given by

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill y_{min}\left(k\right)=& \left\{\begin{array}{c}\hfill \min \left[y_{min}^t\left(k\right),y_{min}^s\left(k\right)\right],k⩽k_{fin}\\ \hfill y_{min}^t\left(k\right),k_{fin}<k⩽k_{end}\end{array}\right.\hfill \\ \hfill y_{max}\left(k\right)=& \left\{\begin{array}{c}\hfill \max \left[y_{max}^t\left(k\right),y_{max}^s\left(k\right)\right],k⩽k_{fin}\\ \hfill y_{max}^t\left(k\right),k_{fin}<k⩽k_{end}\end{array}\right.\hfill \end{array}\end{array}$$

(20)

The definitions of lateral constraints $y_{max}^t$, $y_{min}^t$ and $y_{max}^s$, $y_{min}^s$ are designed to keep $V_H$ within the lane boundaries. A unified formula for these parameters are expressed by

$$\begin{array}{c}\hfill \begin{array}{cc}\hfill y_{min}^g\left(k\right)& =y_l-\frac{w_l}{2}+\frac{w_c}{2}\hfill \\ \hfill y_{max}^g\left(k\right)& =y_l+\frac{w_l}{2}-\frac{w_c}{2}\hfill \end{array}\end{array}$$

(21)

where $y_l$ is the lateral position of the target lane centerline, and $w_c$ is the width of host vehicle, $w_l$ is the width of lane.

Based on the characteristics of vehicle tire dynamic, the constraint of lateral acceleration is related to that of longitudinal acceleration. If we define the limitation of acceleration $a_{dyn}$ from the tire friction dynamic, $a{y}_H$ should conform to the following constraint:

$$\left|a{y}_H\left(k\right)\right|⩽\sqrt{{a_{dyn}}^2-{a{x}_H\left(k\right)}^2}$$

(22)

2.3. Trajectory Re-Planning

For each time instance during the lane change process, the algorithm will check the predicted movement of traffic vehicles until the end of planning horizon. The constraints of traffic gap are then updated to validate the current trajectory. If the planned trajectory exceeds the new gap constraint at any point, a trajectory re-planning needs to be conducted. During the re-planning control, the algorithm evaluates the original and target gap, then attempts to generate a new trajectory by the quadratic programming function. By utilizing the proposed longitudinal and lateral constraints, the time variant safety margin can limit the driving parameters in a safe range, and make sure the re-planning trajectory be collision-free. Since the safety margins $s_F^a$ and $s_R^a$ start from zero for each recalculation, constraints automatically become looser with each time step, so frequent re-planning due to minor constraint violation can be avoided.

After the trajectory re-planning, the host vehicle have two kinds control results: continue the lane change, or abandon the maneuver and return to the original lane. Both of two control results will be conducted by the replanned trajectory. As an effort to avoid abrupt changes in acceleration, we limit the changing rate of both longitudinal and lateral acceleration during the replanning control. The constraint is designed as

$$j_{min}-M_{j1}⩽\frac{a_H(k+1)-a_H\left(k\right)}{t_s}⩽j_{max}+M_{j2}$$

(23)

where $a_H$ refers to either $a{x}_H$ or $a{y}_H$, and the same applies to $j_{min}$, $j_{max}$, $M_{j1}$ and $M_{j2}$. The definitions of these parameters are similar to the Equation (12).

3. Experimental Verification

To verify the performance of the proposed lane change algorithm, this section presents numerical calculation results in a simulation environment. Firstly, in Section 3.1, a simulated traffic flow with each vehicle having random initial position, time varying speed and acceleration is designed for the lane change maneuver. This is used to test the reliability and effectiveness of the algorithm with random traffic situation. Secondly, in Section 3.2, three kinds of specified traffic environment with unexpected road events are employed to test the robustness of proposed method. For simplicity, all traffic vehicles are set with the same size.

Table 2. Predefined Simulation Parameters.

Symbol	Value	Symbol	Value	Symbol	Value
$t_1$	$0.5\mathrm{s}$	$t_2$	$1.0\mathrm{s}$	$a_{dyn}$	$9\mathrm{m}/{\mathrm{s}}^2$
$q_{1x}$	1	$q_{2x}$	10	$q_{3x}$	1
$q_{1y}$	1	$q_{2y}$	10	$q_{3y}$	1
$t_g$	$0.5\mathrm{s}$	$q_{ϵx}$	50	$q_{ϵy}$	50
$K_F$	2	$K_R$	2	$w_l$	$3.5\mathrm{m}$
$w_1$	1	$w_2$	5	$w_3$	$0.1$
$p_1$	$-1.414$	$p_2$	1	$\beta$	$-1$

shows the predefined parameters used in the simulations.

3.1. Simulation with Random Traffic

In order to achieve a dynamic traffic environment for the experiment, a microscopic road traffic simulation is designed. Here Intelligent Driver Model in [22] is employed to generate the traffic flow with vehicle cruise control. The proposed lane change maneuver algorithm is evaluated in a four-lane highway test scene.

At the start of simulation, traffic vehicles are created within certain range from an arbitrary point on the road. Each vehicle is given a random initial speed and target speed by using normal distribution $N\left({\mu }_v,{{\sigma }_v}^2\right)$, and an upper and lower limitation is set to avoid the unrealistic speeds. For The distances between traffic vehicles are initialized with logarithmic normal distribution $N\left({\mu }_t,{{\sigma }_t}^2\right)$. The fitting functions and their parameters ${\mu }_v$, ${\sigma }_v$, ${\mu }_t$ and ${\sigma }_t$ are determined using real road traffic records in [23]. Based on the Intelligent Driver Model, the desired acceleration of traffic vehicles is calculated by their current speed and target speed. The initialized vehicle which has closest longitudinal distance to the origin is selected as the host vehicle $V_H$.

The lane change simulation is evaluated on the 100 versions of the traffic scenarios. Each traffic scenario is simulated with one minute. The random vehicle states in each traffic scenario are generated with the same set of seeds, so the simulation scenarios for different methods can be consistent with each other.

There are two kinds of continuous trajectory planning methods. One is time-based re-planning method (TBRP) which take re-planning algorithm with a constant time interval. A balance should be found between the algorithm complexity and efficiency. Another one is condition-based re-planning (CBRP) method which take re-planning algorithm by a predefined trigger mechanism. This method has less amount of calculation than TBRP, but the final performance needs a good design of control logic.

To verify the effect of the proposed condition-based re-planning (CBRP) algorithm, a time-based re-planning (TBRP) method is compared in the simulation. TBRP represents conventional methods for path planning, which take re-planning algorithm with a constant time interval. This method has good effectiveness and robustness, but the computation complexity is a challenge for the real-time implementation. The CBRP method takes re-planning algorithm by a predefined trigger mechanism. This method has less amount of calculation than TBRP, but the final performance needs a good design of control logic. In order to show the improvement, the comparative indices of the lane change control is defined as: Numbers of LC, Numbers of RP (re-planning), Average LC Time, Average Acceleration, Average Speed and Total Computing Time. Here the computing time is achieved by the simulation time in MATLAB environment.

The results of one hundred random traffic scenarios are presented in

Table 3. Result of Random Traffic Test I.

Method	TBRP	CBRP-C	CBRP-A
Numbers of LC	280	277	283
Numbers of RP	86	20	43
Average LC Time ($\mathrm{s}$)	1.85	1.48	1.40
Average Acceleration ($\mathrm{m}/{\mathrm{s}}^2$)	1.9	1.6	1.7
Average Speed ($\mathrm{m}/\mathrm{s}$)	22.57	22.54	22.55
Total Computing Time ($\mathrm{s}$)	16768	2736	2774

. Since the simulated traffic flow is designed with varying vehicle speed, the target speed of each vehicle is randomly selected within an interval of 15∼30 $\mathrm{m}/\mathrm{s}$, and updates independently per a random time interval between 5 and 20 s. The host vehicle will control to take lane change maneuver, and its target speed is set to $25\mathrm{m}/\mathrm{s}$. The performance of lane change maneuver is compared between TBRP and CBRP. To show the influence of the time-variant safety margin, results of CBRP with a conservative (CBRP-C) and aggressive (CBRP-A) settings are investigated. For CBRP-C, the gain coefficients of safety margin are set as $K_F=K_R=1.0$, while for CBRP-A, are set as $K_F=K_R=0.5$.

Compared to the results of TBRP, the lane change control of CBRP has lower numbers of RP, and smaller average acceleration. This is because TBRP takes the trajectory planning with a constant time interval without the consideration of previous planning results. And CBRP with the time-variant safety margins takes real time planning by monitoring the validity of current trajectory. The robustness of CBRP can reduce the rate of route re-planning and improve the driving comfort in the random traffic flow. Furthermore, the algorithm of CBRP also achieves significant advantage of the computing time. Between the two CBRP methods, CBRP-A has slightly higher numbers of RP and average acceleration than those of CBRP-B. It means that the lane changing behavior can be customized with respect to different habits of drivers.

3.2. Simulation with Specified Traffic

In this section, the proposed algorithm is verified by comparative analysis in the simulations. We set up a typical traffic scenario with two lanes and four traffic vehicles, where the host vehicle $V_H$ take a lane change maneuver. The traffic scenario is shown in Figure 6. To examine the robustness of the proposed algorithm, three typical road events that could happen during a lane change are designed as: unexpected braking of front vehicle in current lane, unexpected braking of front vehicle in target lane, and unexpected acceleration of rear vehicle in target lane. For each scenario, simulations are conducted by different path planning methods: LC with the proposed algorithm, LC without time-variant safety margin, and LC without re-planning control. For comparison purposes, three methods are referred to A, B and C respectively. By comparative analysis, the proposed algorithm’s ability to take appropriate approaches for different given situations is demonstrated.

3.2.1. Scenario I, Unexpected Braking in Current Lane

In this scenario, the front vehicle in current lane $V_{sF}$ take an emergency braking at the same time of lane change. The deceleration of brake is selected as $-2\mathrm{m}/{\mathrm{s}}^2$, $-3\mathrm{m}/{\mathrm{s}}^2$ and $-4\mathrm{m}/{\mathrm{s}}^2$ respectively, and the duration of brake is 3 s. Depending on the lane change algorithm, $V_H$ can choose to increase speed with the potential danger of colliding with $V_{sF}$ or $V_{tR}$, or abandon the current motion and return to the original lane. Simulation results by using different methods A, B and C are compared in

Table 4. Experimental Results of Different Scenarios.

Traffic Scenario	A	B	C
Scenario I,  $-2\mathrm{m}/{\mathrm{s}}^2$	✓	✓	✓
Scenario I,  $-3\mathrm{m}/{\mathrm{s}}^2$	✓	∘	×
Scenario I,  $-4\mathrm{m}/{\mathrm{s}}^2$	∘	∘	×
Scenario II,  $-4\mathrm{m}/{\mathrm{s}}^2$	✓	✓	✓
Scenario II,  $-5\mathrm{m}/{\mathrm{s}}^2$	∘	∘	×
Scenario II,  $-6\mathrm{m}/{\mathrm{s}}^2$	∘	×	×
Scenario III,  $+2\mathrm{m}/{\mathrm{s}}^2$	✓	✓	×
Scenario III,  $+3\mathrm{m}/{\mathrm{s}}^2$	∘	∘	×
Scenario III,  $+4\mathrm{m}/{\mathrm{s}}^2$	∘	×	×

. The check mark with a tick means a successful lane change, a circle means the vehicle abandons the lane change maneuver, and a cross means a collision with other vehicles.

A detailed demonstration with $-4\mathrm{m}/{\mathrm{s}}^2$ emergency braking is shown in Figure 7. In the graphs depicting the velocity and acceleration of host vehicle, the dark shades represent the areas within strict parameter constraints, and the light shades represent those within loose constraints. It can be observed that apart from the longitudinal velocity, other values rarely exceed the strict constraints during simulation, and all values stay within the loose constraints. For the last graph, the upper and lower boundaries of the shaded area obtained by the length of front vehicle and host vehicle, which means any curve that come in contact with the shaded area indicate a collision.

From the results of simulation C in Figure 7, $V_H$ collides with $V_{sF}$ at $t=2.8\mathrm{s}$. In the simulation A and B, $V_H$ abandons the lane change maneuver and switches back to $L_1$ with deceleration. Compared with the results of B, the simulation A has smaller deceleration, faster response to unexpected event and longer safe distance to front vehicle.

3.2.2. Scenario II, Unexpected Braking in Target Lane

In this scenario, the front vehicle in target lane $V_{tF}$ take an emergency brake at the start of lane change. The deceleration of brake is selected as $-4\mathrm{m}/{\mathrm{s}}^2$, $-5\mathrm{m}/{\mathrm{s}}^2$ and $-6\mathrm{m}/{\mathrm{s}}^2$ respectively, and the duration of brake is 3 s. Here, $V_H$ can choose to continue the lane change by reduce the speed, or change back to the original traffic gap and wait for future opportunities. Simulation results are compared in Table 4.

The results with $-6\mathrm{m}/{\mathrm{s}}^2$ emergency braking is shown in Figure 8. In the simulation C, the end speed of $V_H$ after lane change is too fast, causing a collision with $V_{tF}$ at $t=5.15\mathrm{s}$. In the simulation B, $V_H$ attempts to return to original lane $L_1$ after the unexpected event, but fails since it is too close to the front vehicle $V_{sF}$. So $V_H$ can only take a hard brake stop to avoid the accident. In the simulation A, $V_H$ returns to $L_1$ successfully with minimal speed change, and continue the lane change to the traffic gap in the head of $V_{tF}$.

3.2.3. Scenario III, Unexpected Acceleration in Target Lane

In this scenario, the behind vehicle in target lane $V_{tR}$ take an emergency acceleration at the start of lane change. The acceleration of vehicle is selected as $+2\mathrm{m}/{\mathrm{s}}^2$, $+3\mathrm{m}/{\mathrm{s}}^2$ and $+4\mathrm{m}/{\mathrm{s}}^2$ respectively, and the duration of brake is 3 s. Here, $V_H$ can choose to increase the speed to finish the lane change, or return back to the original lane. From the Table 4, simulation A achieved safer results than simulation B and C.

From the detailed results with $+4\mathrm{m}/{\mathrm{s}}^2$ acceleration in Figure 9, both the simulation B and C lead to a collision with $V_{tR}$ at $t=4.65\mathrm{s}$. In the simulation A, due to stricter constraints used during trajectory planning, $V_H$ abandons the lane change maneuver and return to $L_1$ firstly, and then succeeds in doing so before potentially being rear-ended by $V_{tR}$. During the whole progress, the $V_H$ is controlled with minimal speed change.

4. Conclusions

The research proposes a safety and comfort-oriented trajectory planning algorithm for autonomous ground vehicles to perform lane changes on a straight driveway. The algorithms is designed to solve the issues of mutual interactions between the host vehicle and the surrounding cars. In this paper, a complete algorithm flow for lane change maneuver are considered with three parts: gap evaluation, trajectory planning and trajectory correction. A Gray Prediction Model is employed as the gap evaluation method to select an appropriate time instance for lane change. By using the QP-based optimization algorithm, a continuous trajectory planning is designed to achieve both safe and comfortable lane change maneuver. Moreover, a trajectory re-planning control algorithm with condition-triggered safety margin function is proposed. The time-variant safety margins is designed by the consideration of both the longitudinal and the lateral planning in a dynamic traffic. This method can improve the success rate of lane change, and also reduce the computational complexity.

The effectiveness of the algorithms is verified in the simulations with random traffic and specified traffic. By the comparative analysis of the experimental results, the proposed condition-based trajectory re-planning method has a stronger robustness to deal with the unexpected road events, while not inducing too heavy calculated load. By the adjust of time-variant safety margin, the driving model with comfortable or aggressive characteristic can be realized. This opens a potential research topic for the autonomous driving with different individual driver behaviors in future work.