1 Introduction

The rapid development of autonomous driving technology has promoted the application of advanced driver assistance systems in vehicles to improve driving comfort and safety. With precise perception, accurate localization, efficient motion planning, and high-performance computing, it can prevent traffic accidents resulting from driver’s errors and improve driving comfort as well as transportation efficiency [1]. Safety has become a significant consideration factor for the wide implementation of automated vehicles in the transportation. Current driver assistance systems are generally used in relatively simple driving scenarios. It is still challenging for vehicles to finish complex tasks and prevent collisions with stochastic uncertain objects like a skilled human driver. Trajectory planning with collision avoidance is significant to realize safe and comfortable autonomous driving in complex urban environments.

Motion planning is a central part for autonomous driving and mobile robots. It uses data from the perception system to generate a smooth path/trajectory for vehicle tracking and avoid collision with obstacles. Various numerical methods, e.g., graph-search method, incremental tree-based methods, optimization methods, are summarized in Ref. [2]. The path or trajectory should satisfy dynamic feasibility, driving comfort, and collision-free with surroundings. In graph-search methods, the free configuration space is discretized into various cells for the path planning problem. Road network, geometries or sampling in the control space is implemented for the graph construction. A*, hybrid A* and lattice planning have been successfully implemented in the DARPA Urban Challenge in Refs. [3, 4, 5]. Forward propagation with vehicle kinematic models is used to check the collision-free constraints. However, the discretization of the whole configuration space affects the quality considerable, and the problem is generally suboptimal. Unlike focusing on a fixed discretized graph, incremental tree-based methods generate kino-dynamic and collision-free paths by incrementally building a tree. The space is searched iteratively by forward propagation with the kinematic vehicle model. Rapidly exploring random tree-based approaches have been developed for non-holonomic dynamic systems to find feasible paths in Refs. [6, 7, 8]. The exploration is achieved by random sampling in the free configuration space to find a feasible and collision-free path. These methods usually require huge computation for optimal solutions, and the obtained path is generally not smooth for direct tracking.

Optimization-based methods are proposed to optimize some performance index with a set of equality and inequality constraints. The kino-dynamic constraints (e.g., curvature) and physical constraints (e.g., vehicle lateral and longitudinal limits) for non-holonomic systems are incorporated explicitly. The objective function contains the jerk, trip time and tracking errors in Refs. [9, 10, 11, 12]. The collision avoidance constraints are non-convex and nonlinear, which requires nonlinear optimal programming for possible solution. This is usually computational expensive [13, 14]. A nonlinear model predictive control method has been used for the online motion planning of mobile robots in Ref. [15]. A convex inner approximation method is developed to model the collision avoidance constraint. A free ball is used to model the robot, and the shape of the object is limited to a wall. This simplification is not suitable for autonomous vehicles with a more complex shape. In Ref. [16], trajectories for path following under various time-varying constraints are optimized by a linear model predictive control framework along a given reference curve. The vehicles are approximated as three circles, and the distance to the objects is simplified as a linear function of vehicle orientation, velocity and curvature. However, this method requires the feasible reference curvature which introduces additional computation effort and increases complexity. A collision-free trajectory in unstructured environments with static and moving obstacles has been solved with nonlinear model predictive contouring control in Ref. [17]. A polyhedral set with a closed-form bound is used to approximate the collision-free area around the robot, while a dynamically feasible reference path and speed should be provided in advance. The collision avoidance methods for static and dynamic objects are separated, which is not convenient for real-world application.

Motion planning with collision avoidance in complex environments has been studied for mobile robots. Optimization problem is solved to obtain a collision-free trajectory for safe movement in Refs. [15, 17]. Safety is usually formulated as a constraint for the optimization problem using the distance between the robot and the surrounding objects. Such collision avoidance constraint is generally nonlinear and hard to solve. Most research approximates the objects as circles or ellipses and formulate the optimization problem into a nonlinear optimal programming in Refs. [17, 18]. However, the computation is expensive and requires commercial numerical software. It is hard for real-time implementation.

In autonomous driving, safe motion planning is a difficult driving task which involves both longitudinal and lateral movements. Figure 1 presents an autonomous overtaking task in urban driving. The vehicle should drive along the lane of road or reference path and avoid collision with other object vehicles. It includes a sequence of actions under several dynamic and static constraints, such as vehicle speed limit, surrounding objects, traffic conditions and road geometry. An autonomous ego vehicle passes by a slow-moving/static object vehicle ahead through a lane change maneuver to an adjacent lane. After a few seconds of lane-keeping, autonomous ego vehicle (AEV) returns to its original lane and continues driving. During the whole process, a dynamically feasible and collision-free trajectory is necessary for driving comfort and safety. In order to avoid the collisions with other object vehicles, the task has been regarded as a combination of lane change and path tracking maneuvers with two levels in Refs. [19, 20]. In the high-level, behavior planning (e.g., using fuzzy controller, finite state machines) for lane change is decided to give possible path or trajectory. Then, a low-level motion tracking is realized with a polynomial trajectory. The dynamic performances (e.g., jerk and deviation minimization) in the structured road can be applied to optimize the trajectory. In this case, the collision-free constraint is not explicitly involved. Moreover, the lane change action by the behavior planning should be executed in advance for safe driving. Collision avoidance path/trajectory can be solved by the graph-search and incremental tree-based methods, but the dynamic feasibility and comfort may not be guaranteed meanwhile. In Ref. [21], the trajectory planning in structured environments is solved by a robust MPC-based method. A self-defined safe zone based on geometry is used for the collision-free constraint, which is too conservative to be applied in different driving scenarios.

Fig. 1
figure 1

Illustration of autonomous driving with collision avoidance

Full size image

Recently, successive linearization of the non-convex feasible set and nonlinear dynamic constraints is proposed in Ref. [22] to design the motion planning for autonomous driving in unstructured scenarios. The linearization error may introduce infeasibility issues for the optimal solutions at corner cases. Constrained iterative LQR is developed to solve the motion planning problem in Ref. [23]. However, a good initial trajectory is required to solve the optimization problem which is elaborating to design for real-world application particularly in different driving scenarios.

Throughout the literature, there has been little research on a real-time motion planning approach with collision avoidance for optimal trajectory planning in time domain for both longitudinal and lateral movement. A dynamically feasible and collision-free trajectory is necessary for safe and comfortable autonomous driving. Moreover, the robustness in dealing with different complex driving tasks without high-level planning is highly desirable for motion planning to improve the efficiency and avoid communication faults between different layers. Furthermore, existing methods generally approximate obstacles by circles or ellipses. However, this may neglect the geometry and dimension of real vehicles. The formulated optimization problem for trajectory calculation is nonlinear, and the computation is expensive for real-time implementation.

To address the issues discussed above, this paper develops a real-time optimal trajectory planning approach for safe autonomous driving with collision avoidance in complex environments. A novel approximate convex optimization method is proposed to solve the problem for the motion planning as in Fig. 2. The distance between autonomous ego and object vehicles is described with a dual variable and is calculated by solving a dual problem optimization. The objects around AEV are modeled as polygons instead of eclipses or circles for efficient computation. The optimized dual variable is used to formulate the collision-free constraint. The trajectory in longitudinal and lateral directions is further obtained in a model predictive control (MPC) framework for the motion planning with collision avoidance to realize the driving comfort and safety. It can relax the nonlinear collision-free constraints and the nonlinear dynamic model by using the proposed approximate convex optimization method. The formulated optimization problem is convex and can be solved by using efficient quadratic programming (QP) solvers. It can greatly improve the computation efficiency.

Fig. 2
figure 2

Architecture of the approximate convex optimization method

Full size image

In this paper, a computationally efficient method is to be presented for the online optimal collision-free trajectory planning. The objectives of this paper, respectively, are (1) to provide a dynamically feasible and collision-free trajectory in both longitudinal and lateral motions, by taking object vehicles as polygons instead of circles or ellipses; (2) to examine the robustness and capability of handling complex driving environments with multiple object vehicles; (3) to realize fast computation for real-time implementation for final purpose.

This paper is organized as follows. The problem formulation for the optimal motion planning of autonomous driving is introduced in Sect. 2. The approximate convex optimization method is presented in Sect. 3. In Sect. 4, numerical simulations in various driving scenarios are examined to evaluate the performance and robustness. Conclusions are finally given in Sect. 5.

2 Problem Formulation for Trajectory Optimization

In this section, the trajectory optimization problem for motion planning with collision avoidance is introduced. The vehicle motion description is modeled by a kinematic model. Safety and comfort constraints are analyzed. And the nonlinear optimization problem in the MPC framework is constructed.

2.1 Trajectory Optimization

In urban driving cases, vehicles drive along the roads and plan the motion based on scenarios, e.g., traffic rules, static/moving objects, pedestrians. Vehicles should move smoothly in the road network and prevent collisions with the surroundings. The decrease in vehicle speed during driving under some circumstances will affect the driving behaviors of following vehicles and reduce the traffic efficiency. In this case, the following vehicle might choose to overtake the front vehicle and continue to drive with its target speed. A general obstacle avoidance behavior for autonomous driving is shown in Fig. 1, where AEV drives with speed v along the road. There is a slow-moving object vehicle which blocks the road. In order to keep driving, overtaking the object vehicle ahead is necessary. AEV moves to the left lane, passes the object and finally returns to the original lane. The motion planning system designs a dynamically feasible trajectory to drive AEV to fulfill the whole task in a comfortable and safe behavior. It keeps in current driving lane and reduce the disturbances to other vehicles for safety. On the other hand, AEV should avoid collisions to surrounding objects during driving and perform a smooth trajectory. Therefore, the objectives for the trajectory planning includes minimizing the longitudinal and lateral jerks, the deviation from the center lane and curvature change. This planning can be obtained by solving an optimization problem. The resulting sequence of control variables are used to generate the optimal trajectory for motion tracking. The optimization problem is described by:

$$\begin{gathered} \min _{{{\varvec{u}}_{{{t}}} \in \mathbb{R}^{n} }} \int_{0}^{T} {l\left( {{\varvec{x}}_{{{t}}} ,{\varvec{u}}_{{{t}}} } \right)\text dt} \hfill \\ {\text{s}}.{\text{t}}.\;~{\mathbf{\dot{x}}}_{t} = f\left( {{\varvec{x}}_{t} ,{\varvec{u}}_{t} } \right)~ \hfill \\ \left( {{\varvec{x}}_{{{t}}} ,{\varvec{u}}_{{{t}}} } \right) \in \mathbb{Z} \hfill \\ \end{gathered}$$
(1)

where xt ∈ Rm is the system state vector, and ut ∈ Rn is the control input. Detailed descriptions about the state and control variables are given in Sect. 2.2. f is the system dynamics equation, Z is the constraint imposed on the state and input, T is the time duration, and l is the stage cost. The optimal input sequence u ∗  = [u0, u1, …, uT]T is calculated by solving the problem described in Eq. (1).

2.2 System Model

Vehicle models are used for future motion prediction, and various methods have been developed to study the vehicle dynamics and design controllers for different applications [24]. A more in-depth analysis between kinematic and dynamics models are compared for autonomous driving in Ref. [25]. For motion planning with collision avoidance in urban environments, AEV traverses with a relatively low speed and the slip angle of the tire can be ignored. In this paper, a nonlinear kinematic bicycle model is used to predict the future states for simplicity as shown in Fig. 3. If a more accurate vehicle model is developed, it can still be plugged into the following proposed trajectory planning architecture. Bicycle model is used here considering its simplicity because the focus of the research is not on the vehicle modeling.

Fig. 3
figure 3

Nonlinear kinematic model

Full size image

The model can be described by:

$$\begin{gathered} \dot{p}_{x} = v{\text{cos }}\phi \hfill \\ \dot{p}_{y} = v{\text{sin }}\phi \hfill \\ \dot{v} = a \hfill \\ \dot{\phi } = \frac{v}{L}{\text{tan }}\delta \hfill \\ \dot{e} = v{\text{sin }}\varphi \hfill \\ \dot{\varphi } = \frac{v}{L}{\text{tan}}\delta - \dot{\phi }_{{\mathbf{r}}} \hfill \\ \end{gathered}$$
(2)

where px, py are positions of the vehicle at the center point of the real axle in the inertial coordinate system, v, a are speed and acceleration, respectively, φ is the yaw angle, δ is the steering angle, L is the wheelbase, e is the lateral displacement relative to the reference path, φ is the relative yaw angle, and \(\dot\varphi_\text{r}\) is the yaw rate of the reference path. The system state variables are x = [px, py, v, Φ, e, φ]T, and the control inputs are u = [a, δ]T. The continuous-time dynamic model can be summarized as \(\varvec{\dot {x}}\) = f (x, u). By using Euler approximation, the model can be further discretized by:

$${\varvec{x}}_{{k + 1}} = {\varvec{x}}_{k} + f\left( {{\varvec{x}}_{k} ,{\varvec{u}}_{k} } \right){{\Delta }}k$$
(3)

where ∆k is the sampling interval.

2.3 Constraints for Driving Comfort and Safety

In order to realize a kinematically feasible, smooth and collision-free trajectory, some constraints are applied for system states and inputs. AEV should follow the traffic rule and drive only on the permitted area of the road. The position is therefore limited in the available space. Moreover, the vehicle physical limits of steering actuator and traction force restrict the control inputs within certain ranges. Furthermore, in order to avoid aggressive maneuver in both longitudinal and lateral directions for driving comfort, the rates of acceleration and the steering angle are restricted, which can be found in Eq. (8).

During the driving task, AEV should follow its prevailing path and avoid collision with surrounding objects for safety. The collision avoidance is generally modeled by the constraints described by:

$${\mathbb{S}}\left( {{\varvec{x}}_{\varvec{k}} } \right) \cap {\mathbb{O}} = \emptyset$$
(4)

where S(xk) represents the position of AEV, and the space occupied by objects is described by \({\mathbb{O}}\). There should be no intersection space between AEV and objects, and the relative distance should be larger than zero. The distance between AEV and objects is illustrated in Fig. 4 and Eq. (5) as follows:

Fig. 4
figure 4

Illustration of distance between AEV and objects

Full size image
$${\text{dist}}\left( {{\mathbb{S}}\left( {{\varvec{x}}_{k} } \right),{\mathbb{O}}} \right): = \mathop \text{min} \limits_{\varvec{d}} \left\{ {\parallel {\varvec{d}}\parallel ,\left( {{\mathbb{S}}\left( {{\varvec{x}}_{k} } \right) + {\varvec{d}}} \right) \cap {\mathbb{O}} \ne \emptyset } \right\}$$
(5)

The collision avoidance constraints can therefore be presented as:

$$\mathrm{dist}\left({\mathbb{S}}\left({{\varvec{x}}}_{k}\right),{\mathbb{O}}\right)\ge {\varvec{d}}_{\mathrm{min}}$$
(6)

where dmin is the minimum available distance with dmin > 0.

2.4 Cost Function

During the driving process, AEV needs to follow the path with a desired driving speed. To ensure the driving efficiency and avoid unnecessary movement without large displacement from the reference path, lateral displacement e and relative yaw angle φ are required to be close to zero. Therefore, cost function is defined as

$$l\left( {{\varvec{x}}_{k} ,{\varvec{u}}_{k} } \right) = \left\| {{\varvec{x}}_{k} } \right. - \left. {{\varvec{x}}_{{{\text{ref}}}} } \right\|_{{\varvec{Q}}}^{2} + \left\| {{\varvec{u}}_{k} } \right\|_{{\varvec{R}}}^{2}$$
(7)

where xref contains the desired driving speed and the reference path to follow (in most cases the reference path should be the centerline of the lane), which can be predefined in some specific scenarios. For example, the bus usually takes specific route and maintains a steady speed. Q and R are the weighting matrix for states and inputs.

2.5 MPC Problem Formulation

Hence, the trajectory optimization problem for motion planning with collision avoidance is formulated in the MPC form [26] as:

$$\begin{gathered} \min _{{{\mathbf{u}}_{k} \in \mathbb{R}^{n} }} \sum _{{k = 0}}^{{N - 1}} \left( {\left\| {{\varvec{x}}_{k} - \left. {{\varvec{x}}_{{{\text{ref}}}} } \right\|} \right._{\user2{Q}}^{2} + \left\| {{\varvec{u}}_{k} } \right\|_{\user2{R}}^{2} } \right) \hfill \\ + \left\| {{\varvec{x}}_{N} - {\varvec{x}}_{{{\text{ref}}}} } \right\|_{\user2{P}}^{2} \hfill \\ {\text{subject to}} \hfill \\ {\varvec{x}}_{{k + 1}} = {\varvec{x}}_{k} + f\left( {{\varvec{x}}_{k} ,{\varvec{u}}_{k} } \right)\Delta k \hfill \\ {\varvec{x}}_{{\min }} \le {\varvec{x}}_{k} \le {\varvec{x}}_{{\max }} \hfill \\ {\varvec{u}}_{{\min }} \le {\varvec{u}}_{k} \le {\varvec{u}}_{{\max }} \hfill \\ \Delta {\varvec{x}}_{{\min }} \le \Delta {\varvec{x}}_{k} \le \Delta {\varvec{x}}_{{\max }} \hfill \\ \Delta {\varvec{u}}_{{\min }} \le \Delta {\varvec{u}}_{k} \le \Delta {\varvec{u}}_{{\max }} \hfill \\ {\text{dist}}\left( {{\mathbb{E}}\left( {{{x}}_{k} } \right),{\mathbb{O}}} \right) \ge \varvec{d}_{{\min }} \hfill \\ {\varvec{x}}_{0} = {\varvec{x}}\left( 0 \right) \hfill \\ \end{gathered}$$
(8)

where P is the terminal weighting matrix, and x0, x1, …, xN represents the state trajectory. The resulting optimal control sequence u ∗ = [u0,u1, …,uN−1]T in receding horizon N is used to calculate the optimal trajectory for motion planning. It can be seen from Eq. (8) that the problem is modeled in discrete form because in real-world control, the control variable is executed at specific rate.

3 Approximate Convex Optimization Method for Trajectory Planning

In this section, a novel approximate convex optimization method is proposed to solve the nonlinear problem for optimal trajectory planning. The vehicle model is approximated by successive linearization. The object vehicles are described by polygons. The distance between vehicles is calculated by solving a dual problem. The collision-free constraint is then formulated with a optimized dual variable. The trajectory optimization for driving safety and comfort is finally simplified into a convex optimization problem.

3.1 Model Approximation and Constraints Convexity

Given a sequence of states and control inputs (e.g., the optimized variables over the horizon in the last step), the system dynamics model in Eq. (3) can be successively linearized by using Taylor series. The time-varying state space formulation is presented as

$$\varvec{x}_{k+1}={\varvec A}_{k}\varvec{x}_{k}+{\varvec B}_{k}{\varvec u}_{k}+{\varvec C}_{k}$$
(9)

where Ak and Bk are the Jacobian matrix for states and inputs, and Ck is the residual term. Ak, Bk and Ck can be calculated, respectively, by

$${\varvec A}_{k}=\left[\begin{array}{cccccc}1& 0& \mathrm{cos}\phi \mathrm{\Delta}k& -v\mathrm{sin}\phi \mathrm{\Delta}k& 0& 0\\ 0& 1& \mathrm{sin}\phi \mathrm{\Delta}k& v\mathrm{cos}\phi \mathrm{\Delta}k& 0& 0\\ 0& 0& 1& 0& 0& 0\\ 0& 0& \frac{\mathrm{tan}\delta \mathrm{\Delta}k}{L}& 1& 0& 0\\ 0& 0& \mathrm{sin}\varphi \mathrm{\Delta}k& 0& 1& v\mathrm{cos}\varphi \mathrm{\Delta}k\\ 0& 0& \frac{\mathrm{tan}\delta \mathrm{\Delta}k}{L}& 0& 0& 1\end{array}\right],$$
$${\varvec B}_{k}=\left[\begin{array}{cc}0& 0\\ 0& 0\\\Delta k& 0\\ 0& \frac{v\Delta k}{L{\mathrm{cos}}^{2}\delta }\\ 0& 0\\ 0& \frac{v\Delta k}{L{\mathrm{cos}}^{2}\delta }\end{array}\right],{\varvec{C}}_{k}=\left[\begin{array}{c}v\phi \mathrm{sin}\phi \mathrm{\Delta k}\\ -v\phi \mathrm{cos}\phi \mathrm{\Delta k}\\ 0\\ -\frac{v\delta\Delta k}{L{\mathrm{cos}}^{2}\delta }\\ -v\varphi \mathrm{cos}\varphi \mathrm{\Delta k}\\ -\frac{v\delta\Delta k}{L{\mathrm{cos}}^{2}\delta }\end{array}\right]$$

A convex set can be used to describe the space occupied by the objects with

$${\mathbb{O}}^{m}=\left\{{\varvec{z}}\in {\mathbb{R}}^{m}:\varvec{A}_{\text{obs }}^{m}z\le \varvec{b}_{\text{obs }}^{m}\right\},m=\mathrm{1,2},\dots ,M$$
(10)

where m is the index of objects around AEV. It can be seen from above equation that the mathematical expression of polygon is the combination of several linear lines, which is more efficient for optimization and much easier to be converted into a convex problem than traditionally used nonlinear eclipse.

Combining Eq. (5) and Eq. (10), it leads to:

$$\mathrm{dist}\left({\mathbb{S}}\left(\varvec{x}_{k}\right),{\mathbb{O}}^{m}\right):={\mathrm{min}}_{{d}} \left\{\parallel \varvec{d}\parallel :\varvec{A}_{\mathrm{obs}}^{m}\left({\mathbb{S}}\left(\varvec{x}_{k}\right)+\varvec{d}\right)\le \varvec{b}_{\mathrm{obs}}^{m}\right\}$$
(11)

In this paper, dual optimization is used for solving the target constrained optimization problem. Generally speaking, dual optimization incorporates the equation and non-equation constraints into the target cost function that needs to be minimized by using linear expression to approximate the constraints. Then, the problem can be solved like traditional non-constraint optimization problem by setting the derivation of the cost function equal to zero. Based on Ref.[27], the dual problem of minimizing d in Eq. (11) is given by:

$$\begin{array}{*{20}l} {\mathop{\max}\limits_{\varvec{\lambda}} \left( {\varvec{A}_{{{\text{obs}}}}^{m} {\mathbb{S}}\left( {\varvec{x}_{k} } \right) - \varvec{b}_{{{\text{obs}}}}^{m} } \right)^{{\text{T}}} {\varvec{\lambda}}} \hfill \\ {{\text{s.t}}{.}\,\left\| {\varvec{A}_{{{\text{obs}}}}^{{\text{T}}} \lambda } \right\|_{*} \le 1} \hfill \\ {\lambda \ge 0} \hfill \\ \end{array}$$
(12)

where λ is a dual variable for the dual problem of (11), ||.||∗ represents the dual norm. Here, L2 norm is used. Therefore, the distance between AEV and objects is obtained by solving the problem described by Eq. (12).

3.2 Trajectory Optimization with MPC

The trajectory optimization approach is based on two optimization steps. First, the dual variable λ is optimized by solving the dual problem for the minimum distance between AEV and the objects.

Then, the linear MPC is constructed to calculate the states and inputs. The optimized trajectory makes the AEV to track the path and avoid obstacles for driving comfort and safety. The optimization problem is summarized as:

$$\begin{array}{*{20}l} { \mathop \text{min} _{{{\varvec{x}},{\varvec{u}},{\varvec{s}}}} \quad \sum\limits_{k = 0}^{N - 1} \left\| {\varvec{x}_{k} - \varvec{x}_{{{\text{ref}}}} } \right\|^{2} {\varvec{Q}} + \left\| {{\varvec{u}}_{k} } \right\|_{{\varvec{R}}}^{2} + \gamma \sum_{m = 1}^{M} s_{k,m}^{2} } \hfill \\ {\quad \quad \quad \quad + \left\| {\varvec{x}_{N} - {\varvec{x}}_{{{\text{ref}}}} } \right\|_{{\varvec{P}}}^{2} } \hfill \\ {\quad \quad {\text{s.t}}.\quad \varvec{x}_{k + 1} = {\varvec{A}}_{k} {\varvec{x}}_{k} + {\varvec{B}}_{k} {\varvec{u}}_{k} + \varvec{C}_{k} } \hfill \\ {\quad \quad \quad \quad {\varvec{x}}_{{{\text{min}}}} \le x_{k} \le {\varvec{x}}_{{{\text{max}}}} } \hfill \\ {\quad \quad \quad \quad {\varvec{u}}_{{{\text{min}}}} \le {\varvec{u}}_{k} \le {\varvec{u}}_{{{\text{max}}}} } \hfill \\ {\quad \quad \quad \quad {\Delta }{\varvec{u}}_{{{\text{min}}}} \le {\Delta }{\varvec{u}}_{k} \le {\Delta }{\varvec{u}}_{{{\text{max}}}} } \hfill \\ {\quad \quad \quad \quad \left( {\varvec{A}_{{{\text{obs}}}}^{m} {\mathbb{S}}\left( {\varvec{x}_{k} } \right) - \varvec{b}_{{{\text{obs}}}}^{m} } \right)^{{\text{T}}} \lambda_{k}^{*} + s_{k,m} \ge \varvec{d}_{{{\text{min}}}} } \hfill \\ {\quad \quad \quad \quad s_{k,m} \ge 0} \hfill \\ {\quad \quad \quad \quad \varvec{x}_{0} = {\varvec{x}}\left( 0 \right)} \hfill \\ {\quad \quad \quad \quad k = 0,1, \ldots ,N - 1,m = 1,2, \ldots ,M} \hfill \\ \end{array}$$
(13)

where sk,m is a slack variable for the soft constraint, and γ is a coefficient.

4 Numerical Simulation and Discussion

The proposed approximate convex optimization method is examined in various scenarios to evaluate the driving performance. The optimized trajectory drives the AEV to follow the reference path and prevent collisions with other vehicles for a safe and comfortable driving behavior. The simulations are conducted with python language. The general performances of trajectory planning in autonomous overtaking driving is presented. Meanwhile, reference tracking is illustrated for safe driving. Moreover, driving situation with a vehicle sudden cut-in is considered for AEV to avoid collisions for the driving safety. Finally, AEV performs the driving task in complex driving environments and prevents collisions with other surrounding vehicles from adjacent lanes. In order to illustrate the motion and the relative position, vehicles at different time frames are plotted with transparency increased over time, e.g., the more transparent the color, the later a fame in time is. The sampling period for discretization is 0.1 s, and the prediction horizon in MPC is 20 steps which is equivalent to 2 s. The videos of the demonstrated simulations can be found at https://youtu.be/VGvQMX68GAY.

4.1 General Performance Evaluation

AEV traverses with constant velocity, and in front of it, there is a static vehicle which stops and blocks the driving road. AEV overtakes the static vehicle and continues driving with its target velocity. The simulation results are shown in Fig. 5. It can be seen that AEV overtakes the static vehicle safely without collision. Trajectories of the lateral displacement, the velocity and the yaw angle are presented as well. When AEV is close to the static vehicle, it changes the yaw angle and moves to the left driving lane. The lateral displacement is increased to avoid collision. During the changing process, the velocity decreases a little at first to keep safe relative distance and lead to a smooth movement. After AEV passes by, the lateral displacement is decreased, and the yaw angle increases to zero. AEV returns to its original driving lane finally. In order to avoid collisions, AEV first reduces the velocity and then performs a larger steering angle to overtake the object vehicle for safety. The proposed method provides a more conservative behavior in terms of safety compared to the baseline which is solved by nonlinear optimization method.

Fig. 5
figure 5

Simulation for the obstacle avoidance

Full size image

4.2 Curve Path Tracking with Collision Avoidance

In this case, AEV drives with 4 m/s and tracks a predefined curve path which can be determined by standard path planning method. The path is represented by black waypoints. Two static red objects block the path unexpectedly after the path has been calculated for vehicle tracking. AEV should follow the path and avoid collisions with the red objects. The simulation results are presented in Fig. 6. The position of AEV over different time frames are illustrated. It can be seen that AEV can track the path and prevent collision with red objects successfully. The minimum clearance to the obstacles is 0.48 and 0.65 m, respectively, implying safe driving without collisions. The comparison of lateral displacement, velocity and yaw angle is shown in Fig. 6. It can be seen that the trajectory in the lateral direction is smooth both with and without obstacles, contributing to comfortable driving. AEV can follow the path closely, and the lateral displacement has a good match with the waypoints. When AEV gets close to objects, it changes the lateral movement to overtake objects and returns to the original path. During the whole process, the velocity keeps nearly unchanged for efficient path tracking performance. The yaw angle has similar profiles. The mismatch happens when AEV overtakes the object to prevent collisions.

Fig. 6
figure 6

Simulation for curve path tracking with obstacle avoidance

Full size image

4.3 Vehicle Sudden Cut-in

Due to some unexpected human driving behaviors, vehicles from adjacent lanes may conduct sudden lane change. Such driving maneuver is not predicted in advance and would cause danger to the vehicles behind as the distance is very short, which limits the reaction time for hard braking. In order to avoid collision, the following vehicle should execute evasive turning and overtake the cut-in vehicle. In this scenario, a blue AEV traverses with 10 m/s, and in the same lane, there is a red vehicle with velocity of 3 m/s. At 30 m ahead, a cyan vehicle drives in an adjacent lane. It violently cuts in the lane of AEV and follows a fifth-order polynomial trajectory. In order to continue driving, AEV should at first overtake the red vehicle. When it detects the movement of the cut-in cyan vehicle after passing by the red vehicle, the AEV needs to change the steering angle to avoid collisions with the cyan vehicle. The simulation results are shown in Fig. 7, which presents the movements of the three vehicles in different time frames. It is seen that AEV can prevent collisions with both red and cyan vehicles successfully when the object vehicle performs a sudden lane change maneuver. The trajectories of lateral displacement, velocity and yaw angle are shown. Here, the situation where there is only red vehicle is used as comparison. Before 30 m, both two cases have similar trajectory in terms of lateral displacement and yaw angle. During this period, AEV overtakes the red vehicle. When AEV detects the cyan vehicle cutting into the driving lane, it increases its lateral displacement through performing a yielding turn to pass the cyan vehicle and avoid collisions. When there is enough distance between two vehicles, it returns to the original driving lane. Therefore, the lateral displacement and yaw angle converge eventually. The velocity of AEV remains unchanged so as to finish the overtaking process as soon as possible. This can reduce disturbance to vehicles in other lanes. The yaw angle trajectory maintains smooth to ensure driving comfort.

Fig. 7
figure 7

Simulation for a vehicle sudden cut-in

Full size image

4.4 Object Vehicles Driving with Different Velocities

During autonomous urban driving, in order to prevent collisions with front vehicles, AEV moves to its adjacent lane to pass the vehicle and returns when there is safe distance with surrounding vehicles. Meanwhile, it should not influence the driving of surrounding vehicles in other lanes, and prevent collisions with them. In this scenario, two object vehicles driving in the adjacent lanes with the same direction are considered in Fig. 8. The AEV in blue drives on the same lane with the red vehicle. The cyan vehicle drives faster on the adjacent lane. AEV drives to prevent collisions with both red and cyan vehicles. The simulation results indicate that AEV can avoid collision with both two object vehicles successfully and perform efficient driving. Trajectories of lateral movement, velocity and yaw angle are illustrated. When AEV gets close to the red vehicle and prepares to turn to the adjacent lane, the cyan vehicle drives close to AEV. At the moment, there is no enough space for AEV to change the driving lane. AEV would first reduce its velocity to prevent collisions with the red vehicle. After the cyan vehicle drives away and it is empty on the adjacent lane, AEV starts to perform a yielding turning to pass the red vehicle. Compared with the situation with one object vehicle, it can be seen that with two object vehicles, AEV has more velocity decrease for the trajectory planning to prevent collisions. The delay in the lateral displacement and the yaw angle indicates that AEV behaves to overtake the red vehicle later for safe driving.

Fig. 8
figure 8

Simulation for obstacle avoidance when two object vehicles drive with different velocity

Full size image

4.5 Object Vehicles Driving with Same Velocity

In this scenario, AEV drives with velocity of 10 m/s along the road. In front of it, there are two object vehicles traversing parallel at 3 m/s in sequence which blocked the road. The inter-distance between them is 13 m. AEV should overtake them one by one and continues to drive on its way. Figure 9 presents that the simulation results of this case show the relative position between AEV and object vehicles over different time frames. At the beginning, AEV steers to the left side to overtake the closest red object vehicle. Then, it turns to the right side and prevents collisions with the cyan vehicle. After AEV passes by the two vehicles, it returns to the original driving path. The trajectories of lateral displacement, velocity and yaw angle are presented. It is compared with the condition when there is only red vehicle in front. The velocity keeps unchanged during the whole process for efficient driving performance. The trajectories of lateral displacement and yaw angle are smooth, enabling comfortable driving. In order to overtake the cyan vehicle, the yaw angle is reduced further to increase the lateral distance to the cyan vehicle after 3 s. This is also reflected in the lateral displacement where the condition with two obstacles has smaller value than the one with one obstacle between 30 and 65 m. Finally, it returns back completely and AEV continues to drive in its original direction.

Fig. 9
figure 9

Simulation for obstacle avoidance when two object vehicles traversing with same velocity

Full size image

Therefore, it is demonstrated that the proposed method can handle different driving scenarios in complex environments without collisions with multiple objects for safe and comfortable driving.

5 Conclusions

This paper proposes a computationally efficient optimal trajectory planning method for the autonomous driving with collision avoidance. A novel approximate convex optimization method is developed and implemented to obtain a dynamic feasible and collision-free trajectory for both longitudinal and lateral motions. The optimal trajectory is calculated in a MPC framework for safe and comfortable driving. The collision-free constraint is formulated with the dual variable by solving the dual problem optimization. The formulated optimization problem is convex and can be solved using efficient QP solvers. Simulation results have shown that the approach reaches similar performance with less computation burden compared to a nonlinear optimization method. The generated trajectory is more conservative in terms of driving safety. Various driving scenarios are examined to evaluate the robustness and the ability for handling complex driving tasks with multiple object vehicles.

In future work, the proposed method will be extended to study the trajectory planning with collision avoidance at traffic intersections with uncertain stochastic dynamic objects. More sophisticated scenarios reflecting real-world traffic states will be incorporated both in simulation and field tests to investigate the effectiveness and robustness of the proposed method. In addition, the proposed method could be improved to suit vehicle with much higher speed limit.